From c8134c6f72ab038ca85509a2b98cf2329661e15e Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 18 Dec 2023 15:54:09 +0100 Subject: [PATCH 01/83] Edited to test sending of logging data to YARP port --- .../src/YarpRobotLoggerDevice.cpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index c94c9549ae..9030e6875e 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -32,6 +32,9 @@ #include #include #include +#include + +#include #include #include @@ -106,6 +109,7 @@ YarpRobotLoggerDevice::~YarpRobotLoggerDevice() = default; bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) { + constexpr auto logPrefix = "[YarpRobotLoggerDevice::open]"; auto params = std::make_shared(config); @@ -1194,6 +1198,17 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit void YarpRobotLoggerDevice::run() { + static bool init = false; + static yarp::os::BufferedPort outPort; + + if (!init) + { + init = true; + outPort.open("/testLoggerOutput"); + } + + yarp::os::Bottle&out = outPort.prepare(); + out.clear(); constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; // get the data @@ -1206,24 +1221,32 @@ void YarpRobotLoggerDevice::run() std::lock_guard lock(m_bufferManagerMutex); // collect the data + std::stringstream dataStream; if (m_streamJointStates) { if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) { + dataStream << "Joint Positions: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::positions"); } if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) { + dataStream << "Joint Velocities: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::velocities"); } if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) { + dataStream << "Joint Accelerations: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::accelerations"); } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { + dataStream << "Joint Torques: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::torques"); } + out.addString(dataStream.str()); + outPort.write(true); + } if (m_streamMotorStates) From 76c01a21ccda447c57866d01d3e6ad1508fd3c89 Mon Sep 17 00:00:00 2001 From: Nick Date: Fri, 29 Dec 2023 20:43:04 +0100 Subject: [PATCH 02/83] Added a test for streaming data, it was successful --- .../src/YarpRobotLoggerDevice.cpp | 58 ++++++++++++++++++- 1 file changed, 55 insertions(+), 3 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 9030e6875e..4d4f4ab4f3 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1224,6 +1224,7 @@ void YarpRobotLoggerDevice::run() std::stringstream dataStream; if (m_streamJointStates) { + yInfo() << "Sending joint states"; if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) { dataStream << "Joint Positions: " << m_jointSensorBuffer << "|"; @@ -1249,64 +1250,86 @@ void YarpRobotLoggerDevice::run() } +// / dataStream.str(std::string()); if (m_streamMotorStates) { if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { + dataStream << "Motor State Positions: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::positions"); } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { + dataStream << "Motor State Velocities: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::velocities"); } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { + dataStream << "Motor State Accelerations: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::accelerations"); } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { + dataStream << "Motor State Currents: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::currents"); } + out.addString(dataStream.str()); + outPort.write(true); } if (m_streamMotorPWM) { if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) { + dataStream << "Motor State PWMs: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::PWM"); } + out.addString(dataStream.str()); + outPort.write(true); } if (m_streamPIDs) { if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) { + dataStream << "PIDs: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "PIDs"); } + out.addString(dataStream.str()); + outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { + dataStream << "FTs::" << sensorName << ": " << m_ftBuffer << "|"; m_bufferManager.push_back(m_ftBuffer, time, "FTs::" + sensorName); } + out.addString(dataStream.str()); + outPort.write(true); } for (const auto& sensorname : m_robotSensorBridge->getTemperatureSensorsList()) { if (m_robotSensorBridge->getTemperature(sensorname, m_ftTemperatureBuffer)) { + dataStream << "temperatures:: " << sensorname << ": " << m_ftTemperatureBuffer << "|"; m_bufferManager.push_back({m_ftTemperatureBuffer}, time, "temperatures::" + sensorname); } + out.addString(dataStream.str()); + outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { + dataStream << "gyros:: " << sensorName << ": " << m_gyroBuffer << "|"; m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); } + out.addString(dataStream.str()); + outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) @@ -1314,24 +1337,34 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) { + dataStream << "accelerometers:: " << sensorName << ": " << m_acceloremeterBuffer << "|"; m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); } + out.addString(dataStream.str()); + outPort.write(true); + } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) { + dataStream << "orientations:: " << sensorName << ": " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); } + out.addString(dataStream.str()); + outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { + dataStream << "magnetometers:: " << sensorName << ": " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_magnemetometerBuffer, time, "magnetometers::" + sensorName); } + out.addString(dataStream.str()); + outPort.write(true); } // an IMU contains a gyro accelerometer and an orientation sensor @@ -1345,18 +1378,28 @@ void YarpRobotLoggerDevice::run() m_gyroBuffer, m_orientationBuffer); + dataStream << "accelerometers::" << sensorName << ": " << m_acceloremeterBuffer << "|"; m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); + + dataStream << "gyros::" << sensorName << ": " << m_gyroBuffer << "|"; m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); + + dataStream << "orientations::" << sensorName << ": " << m_orientationBuffer << "|"; m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); } + out.addString(dataStream.str()); + outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getCartesianWrenchesList()) { if (m_robotSensorBridge->getCartesianWrench(sensorName, m_ftBuffer)) { + dataStream << "caresian_wrenches::" << sensorName << ": " << m_ftBuffer << "|"; m_bufferManager.push_back(m_ftBuffer, time, "cartesian_wrenches::" + sensorName); } + out.addString(dataStream.str()); + outPort.write(true); } std::string signalFullName; @@ -1378,10 +1421,13 @@ void YarpRobotLoggerDevice::run() for (const auto& [key, vector] : collection->vectors) { - signalFullName = signal.signalName + "::" + key; +// signalFullName = signal.signalName + "::" + key; +// dataStream << "signalFullName: " << vector << "|"; m_bufferManager.push_back(vector, time, signalFullName); } } +// out.addString(dataStream.str()); +// outPort.write(true); } for (auto& [name, signal] : m_vectorSignals) @@ -1395,8 +1441,11 @@ void YarpRobotLoggerDevice::run() m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); signal.dataArrived = true; } - m_bufferManager.push_back(*vector, time, signal.signalName); +// dataStream << signal.signalName << ": " << *vector << "|"; +// m_bufferManager.push_back(*vector, time, signal.signalName); } +// out.addString(dataStream.str()); +// outPort.write(true); } int bufferportSize = m_textLoggingPort.getPendingReads(); @@ -1424,8 +1473,11 @@ void YarpRobotLoggerDevice::run() m_textLogsStoredInManager.insert(signalFullName); } - m_bufferManager.push_back(msg, time, signalFullName); +// dataStream << signalFullName << ": " << msg << "|"; +// m_bufferManager.push_back(msg, time, signalFullName); } +// out.addString(dataStream.str()); +// outPort.write(true); bufferportSize = m_textLoggingPort.getPendingReads(); } else { From 4267db2af2244b1171fb4e784d2829bef298a94a Mon Sep 17 00:00:00 2001 From: Nick Date: Sun, 31 Dec 2023 09:59:18 +0100 Subject: [PATCH 03/83] Added dummy value to send for testing --- .../src/YarpRobotLoggerDevice.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 4d4f4ab4f3..d7b4b1c080 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1222,6 +1222,24 @@ void YarpRobotLoggerDevice::run() std::lock_guard lock(m_bufferManagerMutex); // collect the data std::stringstream dataStream; + static int dummyData = 0; + static bool increase = true; + if (increase) + { + dummyData++; + increase = dummyData <= 200; + } + else + { + dummyData--; + increase = dummyData <= 1; + } + + std::cout << std::fixed << time << std::endl; + dataStream << std::fixed << "{\"robot_realtime\":{\"FTs\":{\"l_arm_ft\":{\"data\": [" << dummyData << "], \"timestamps\": [" << time << "]}}}}"; + yInfo() << dataStream.str(); + out.addString(dataStream.str()); + outPort.write(true); if (m_streamJointStates) { yInfo() << "Sending joint states"; From 45bade1cc667b916e1675187397c4a86215eef34 Mon Sep 17 00:00:00 2001 From: Nick Date: Sun, 31 Dec 2023 09:59:58 +0100 Subject: [PATCH 04/83] temporarily removed sending all of the data for testing purposes --- .../src/YarpRobotLoggerDevice.cpp | 88 +++++++++---------- 1 file changed, 44 insertions(+), 44 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index d7b4b1c080..f037cdfb13 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1245,26 +1245,26 @@ void YarpRobotLoggerDevice::run() yInfo() << "Sending joint states"; if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) { - dataStream << "Joint Positions: " << m_jointSensorBuffer << "|"; + // dataStream << "Joint Positions: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::positions"); } if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) { - dataStream << "Joint Velocities: " << m_jointSensorBuffer << "|"; + // dataStream << "Joint Velocities: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::velocities"); } if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) { - dataStream << "Joint Accelerations: " << m_jointSensorBuffer << "|"; + // dataStream << "Joint Accelerations: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::accelerations"); } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { - dataStream << "Joint Torques: " << m_jointSensorBuffer << "|"; + // dataStream << "Joint Torques: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::torques"); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } @@ -1274,80 +1274,80 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { dataStream << "Motor State Positions: " << m_jointSensorBuffer << "|"; - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::positions"); + //m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::positions"); } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { dataStream << "Motor State Velocities: " << m_jointSensorBuffer << "|"; - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::velocities"); + //m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::velocities"); } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { dataStream << "Motor State Accelerations: " << m_jointSensorBuffer << "|"; - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::accelerations"); + //m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::accelerations"); } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { dataStream << "Motor State Currents: " << m_jointSensorBuffer << "|"; - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::currents"); + //m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::currents"); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } if (m_streamMotorPWM) { if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) { - dataStream << "Motor State PWMs: " << m_jointSensorBuffer << "|"; + //dataStream << "Motor State PWMs: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::PWM"); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } if (m_streamPIDs) { if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) { - dataStream << "PIDs: " << m_jointSensorBuffer << "|"; + //dataStream << "PIDs: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "PIDs"); } - out.addString(dataStream.str()); - outPort.write(true); + // out.addString(dataStream.str()); + // outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { - dataStream << "FTs::" << sensorName << ": " << m_ftBuffer << "|"; + //dataStream << "FTs::" << sensorName << ": " << m_ftBuffer << "|"; m_bufferManager.push_back(m_ftBuffer, time, "FTs::" + sensorName); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } for (const auto& sensorname : m_robotSensorBridge->getTemperatureSensorsList()) { if (m_robotSensorBridge->getTemperature(sensorname, m_ftTemperatureBuffer)) { - dataStream << "temperatures:: " << sensorname << ": " << m_ftTemperatureBuffer << "|"; + //dataStream << "temperatures:: " << sensorname << ": " << m_ftTemperatureBuffer << "|"; m_bufferManager.push_back({m_ftTemperatureBuffer}, time, "temperatures::" + sensorname); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { - dataStream << "gyros:: " << sensorName << ": " << m_gyroBuffer << "|"; + //dataStream << "gyros:: " << sensorName << ": " << m_gyroBuffer << "|"; m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) @@ -1355,11 +1355,11 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) { - dataStream << "accelerometers:: " << sensorName << ": " << m_acceloremeterBuffer << "|"; + //dataStream << "accelerometers:: " << sensorName << ": " << m_acceloremeterBuffer << "|"; m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } @@ -1367,22 +1367,22 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) { - dataStream << "orientations:: " << sensorName << ": " << m_jointSensorBuffer << "|"; + //dataStream << "orientations:: " << sensorName << ": " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { - dataStream << "magnetometers:: " << sensorName << ": " << m_jointSensorBuffer << "|"; + //dataStream << "magnetometers:: " << sensorName << ": " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_magnemetometerBuffer, time, "magnetometers::" + sensorName); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } // an IMU contains a gyro accelerometer and an orientation sensor @@ -1396,28 +1396,28 @@ void YarpRobotLoggerDevice::run() m_gyroBuffer, m_orientationBuffer); - dataStream << "accelerometers::" << sensorName << ": " << m_acceloremeterBuffer << "|"; + //dataStream << "accelerometers::" << sensorName << ": " << m_acceloremeterBuffer << "|"; m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); - dataStream << "gyros::" << sensorName << ": " << m_gyroBuffer << "|"; + //dataStream << "gyros::" << sensorName << ": " << m_gyroBuffer << "|"; m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); - dataStream << "orientations::" << sensorName << ": " << m_orientationBuffer << "|"; + //dataStream << "orientations::" << sensorName << ": " << m_orientationBuffer << "|"; m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getCartesianWrenchesList()) { if (m_robotSensorBridge->getCartesianWrench(sensorName, m_ftBuffer)) { - dataStream << "caresian_wrenches::" << sensorName << ": " << m_ftBuffer << "|"; + //dataStream << "caresian_wrenches::" << sensorName << ": " << m_ftBuffer << "|"; m_bufferManager.push_back(m_ftBuffer, time, "cartesian_wrenches::" + sensorName); } - out.addString(dataStream.str()); - outPort.write(true); + //out.addString(dataStream.str()); + //outPort.write(true); } std::string signalFullName; From c213ed7ee333448925c45870a9b4bb182dbde89c Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 2 Jan 2024 09:14:42 +0100 Subject: [PATCH 05/83] Sending FT data to log visualizer --- .../src/YarpRobotLoggerDevice.cpp | 23 +++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index f037cdfb13..8efd0634ad 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1236,10 +1236,10 @@ void YarpRobotLoggerDevice::run() } std::cout << std::fixed << time << std::endl; - dataStream << std::fixed << "{\"robot_realtime\":{\"FTs\":{\"l_arm_ft\":{\"data\": [" << dummyData << "], \"timestamps\": [" << time << "]}}}}"; - yInfo() << dataStream.str(); - out.addString(dataStream.str()); - outPort.write(true); + //dataStream << std::fixed << "{\"robot_realtime\":{\"FTs\":{\"l_arm_ft\":{\"data\": [" << dummyData << "], \"timestamps\": [" << time << "]}}}}"; +// yInfo() << dataStream.str(); +// out.addString(dataStream.str()); +// outPort.write(true); if (m_streamJointStates) { yInfo() << "Sending joint states"; @@ -1317,16 +1317,31 @@ void YarpRobotLoggerDevice::run() // outPort.write(true); } + dataStream << std::fixed << "{\"robot_realtime\":{\"FTs\":{"; for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { //dataStream << "FTs::" << sensorName << ": " << m_ftBuffer << "|"; m_bufferManager.push_back(m_ftBuffer, time, "FTs::" + sensorName); + // std::cout << sensorName << std::endl; + // std::cout << m_ftBuffer << std::endl; + dataStream << "\"" << sensorName << "\":{\"data\": ["; + for (unsigned int i = 0; i < m_ftBuffer.rows() - 1; i++) + dataStream << m_ftBuffer.coeff(i, 0) << ","; + dataStream << m_ftBuffer.coeff(m_ftBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\": [\"f_x\", \"f_y\", \"f_z\", \"mu_x\", \"mu_y\", \"mu_z\"]},"; + } //out.addString(dataStream.str()); //outPort.write(true); } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "}}}"; + out.addString(dataStream.str()); + outPort.write(true); + std::cout << dataStream.str() << std::endl; + dataStream.str(std::string()); for (const auto& sensorname : m_robotSensorBridge->getTemperatureSensorsList()) { From c4da8b5af95a763d4d1ff9c4432e49f81743686d Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 2 Jan 2024 15:03:53 +0100 Subject: [PATCH 06/83] Moved the code for sending data to a more centralized function --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 7 + .../src/YarpRobotLoggerDevice.cpp | 185 +++++++++++------- 2 files changed, 121 insertions(+), 71 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index c1153a1315..20c6c0bfbd 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -157,6 +157,10 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::vector m_textLoggingSubnames; std::vector m_codeStatusCmdPrefixes; + // for realtime data streaming + bool streamRealTime{true}; + yarp::os::BufferedPort realtimeLoggingPort; + std::mutex m_bufferManagerMutex; robometry::BufferManager m_bufferManager; @@ -179,6 +183,9 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::unordered_map>& imgDimensions); bool createFramesFolder(std::shared_ptr imageSaver, const std::string& camera, const std::string& imageType); + + // for realtime data streaming + void SendDataToLoggerVisualizer(); }; } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 8efd0634ad..896ab6cc9f 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -91,6 +91,10 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, // the logging message are streamed using yarp BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); + + // for realtime logging + if (streamRealTime) + realtimeLoggingPort.open("/testLoggerOutput"); } YarpRobotLoggerDevice::YarpRobotLoggerDevice() @@ -103,6 +107,9 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() // the logging message are streamed using yarp BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); + + if (streamRealTime) + realtimeLoggingPort.open("/testLoggerOutput"); } YarpRobotLoggerDevice::~YarpRobotLoggerDevice() = default; @@ -961,6 +968,8 @@ void YarpRobotLoggerDevice::unpackIMU(Eigen::Ref signal, void YarpRobotLoggerDevice::lookForExogenousSignals() { + std::cout << "Looking for exogenous signals" << std::endl; + yarp::profiler::NetworkProfiler::ports_name_set yarpPorts; using namespace std::chrono_literals; @@ -1022,6 +1031,7 @@ bool YarpRobotLoggerDevice::hasSubstring(const std::string& str, void YarpRobotLoggerDevice::lookForNewLogs() { + std::cout << "Looking for new logs" << std::endl; using namespace std::chrono_literals; yarp::profiler::NetworkProfiler::ports_name_set yarpPorts; constexpr auto textLoggingPortPrefix = "/log/"; @@ -1196,19 +1206,101 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit } } -void YarpRobotLoggerDevice::run() +void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() { - static bool init = false; - static yarp::os::BufferedPort outPort; - - if (!init) + const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); + + yarp::os::Bottle& realtimeOutput = realtimeLoggingPort.prepare(); + realtimeOutput.clear(); + std::stringstream dataStream; + dataStream << std::fixed << "{\"robot_realtime\":{"; + if (m_streamJointStates) { - init = true; - outPort.open("/testLoggerOutput"); + bool dictionaryInitialized = false; + std::vector joints; + m_robotSensorBridge->getJointsList(joints); + dataStream << "\"joints_state\":{ "; + // pack the data for the joint positions + if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) + { + dataStream << "\"positions\":{\"data\":["; + for (unsigned int i = 0; i < m_jointSensorBuffer.rows() - 1; i++) + dataStream << m_jointSensorBuffer.coeff(i, 0) << ","; + dataStream << m_jointSensorBuffer.coeff(m_jointSensorBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\":["; + for(unsigned int i = 0; i < joints.size() - 1; i++) + dataStream << "\"" << joints[i] << "\","; + dataStream << "\"" << joints[joints.size() - 1] << "\"]},"; + } + if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) + { + dataStream << "\"velocities\":{\"data\":["; + for (unsigned int i = 0; i < m_jointSensorBuffer.rows() - 1; i++) + dataStream << m_jointSensorBuffer.coeff(i, 0) << ","; + dataStream << m_jointSensorBuffer.coeff(m_jointSensorBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\":["; + for(unsigned int i = 0; i < joints.size() - 1; i++) + dataStream << "\"" << joints[i] << "\","; + dataStream << "\"" << joints[joints.size() - 1] << "\"]},"; + + } + if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) + { + dataStream << "\"accelerations\":{\"data\":["; + for (unsigned int i = 0; i < m_jointSensorBuffer.rows() - 1; i++) + dataStream << m_jointSensorBuffer.coeff(i, 0) << ","; + dataStream << m_jointSensorBuffer.coeff(m_jointSensorBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\":["; + for(unsigned int i = 0; i < joints.size() - 1; i++) + dataStream << "\"" << joints[i] << "\","; + dataStream << "\"" << joints[joints.size() - 1] << "\"]},"; + + } + if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) + { + dataStream << "\"torques\":{\"data\":["; + for (unsigned int i = 0; i < m_jointSensorBuffer.rows() - 1; i++) + dataStream << m_jointSensorBuffer.coeff(i, 0) << ","; + dataStream << m_jointSensorBuffer.coeff(m_jointSensorBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\":["; + for(unsigned int i = 0; i < joints.size() - 1; i++) + dataStream << "\"" << joints[i] << "\","; + dataStream << "\"" << joints[joints.size() - 1] << "\"]},"; + + } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "},"; } + if (m_streamFTSensors) + { + dataStream << "\"FTs\":{"; + for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) + { + if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) + { + dataStream << "\"" << sensorName << "\":{\"data\": ["; + for (unsigned int i = 0; i < m_ftBuffer.rows() - 1; i++) + dataStream << m_ftBuffer.coeff(i, 0) << ","; + dataStream << m_ftBuffer.coeff(m_ftBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\": [\"f_x\", \"f_y\", \"f_z\", \"mu_x\", \"mu_y\", \"mu_z\"]},"; - yarp::os::Bottle&out = outPort.prepare(); - out.clear(); + } + } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "},"; + } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "}}"; + std::cout << std::endl; + std::cout << dataStream.str() << std::endl; + std::cout << std::endl; + realtimeOutput.addString(dataStream.str()); + realtimeLoggingPort.write(true); + +} + +void YarpRobotLoggerDevice::run() +{ constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; // get the data @@ -1220,76 +1312,36 @@ void YarpRobotLoggerDevice::run() const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); std::lock_guard lock(m_bufferManagerMutex); - // collect the data - std::stringstream dataStream; - static int dummyData = 0; - static bool increase = true; - if (increase) - { - dummyData++; - increase = dummyData <= 200; - } - else - { - dummyData--; - increase = dummyData <= 1; - } - std::cout << std::fixed << time << std::endl; - //dataStream << std::fixed << "{\"robot_realtime\":{\"FTs\":{\"l_arm_ft\":{\"data\": [" << dummyData << "], \"timestamps\": [" << time << "]}}}}"; -// yInfo() << dataStream.str(); -// out.addString(dataStream.str()); -// outPort.write(true); if (m_streamJointStates) { - yInfo() << "Sending joint states"; if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) - { - // dataStream << "Joint Positions: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::positions"); - } if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) - { - // dataStream << "Joint Velocities: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::velocities"); - } if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) - { - // dataStream << "Joint Accelerations: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::accelerations"); - } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) - { - // dataStream << "Joint Torques: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::torques"); - } - //out.addString(dataStream.str()); - //outPort.write(true); - } -// / dataStream.str(std::string()); if (m_streamMotorStates) { if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { - dataStream << "Motor State Positions: " << m_jointSensorBuffer << "|"; - //m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::positions"); + // dataStream << "Motor State Positions: " << m_jointSensorBuffer << "|"; } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { - dataStream << "Motor State Velocities: " << m_jointSensorBuffer << "|"; - //m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::velocities"); + // dataStream << "Motor State Velocities: " << m_jointSensorBuffer << "|"; } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { - dataStream << "Motor State Accelerations: " << m_jointSensorBuffer << "|"; - //m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::accelerations"); + // dataStream << "Motor State Accelerations: " << m_jointSensorBuffer << "|"; } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { - dataStream << "Motor State Currents: " << m_jointSensorBuffer << "|"; - //m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::currents"); + // dataStream << "Motor State Currents: " << m_jointSensorBuffer << "|"; } //out.addString(dataStream.str()); //outPort.write(true); @@ -1317,31 +1369,21 @@ void YarpRobotLoggerDevice::run() // outPort.write(true); } - dataStream << std::fixed << "{\"robot_realtime\":{\"FTs\":{"; for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { - //dataStream << "FTs::" << sensorName << ": " << m_ftBuffer << "|"; m_bufferManager.push_back(m_ftBuffer, time, "FTs::" + sensorName); - // std::cout << sensorName << std::endl; - // std::cout << m_ftBuffer << std::endl; - dataStream << "\"" << sensorName << "\":{\"data\": ["; - for (unsigned int i = 0; i < m_ftBuffer.rows() - 1; i++) - dataStream << m_ftBuffer.coeff(i, 0) << ","; - dataStream << m_ftBuffer.coeff(m_ftBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\": [\"f_x\", \"f_y\", \"f_z\", \"mu_x\", \"mu_y\", \"mu_z\"]},"; - } - //out.addString(dataStream.str()); - //outPort.write(true); } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "}}}"; - out.addString(dataStream.str()); - outPort.write(true); - std::cout << dataStream.str() << std::endl; - dataStream.str(std::string()); + // dataStream.seekp(-1, std::ios_base::end); + // dataStream << "}}}"; + // out.addString(dataStream.str()); + // outPort.write(true); + // std::cout << std::endl; + // std::cout << dataStream.str() << std::endl; + // std::cout << std::endl; + // dataStream.str(std::string()); for (const auto& sensorname : m_robotSensorBridge->getTemperatureSensorsList()) { @@ -1517,6 +1559,7 @@ void YarpRobotLoggerDevice::run() break; } } + SendDataToLoggerVisualizer(); } bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, From 46674330713b679c5a71172823e9058e2af51840 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 2 Jan 2024 15:56:02 +0100 Subject: [PATCH 07/83] Added more logging data to send to the visualizer --- .../src/YarpRobotLoggerDevice.cpp | 89 ++++++++++++++++++- 1 file changed, 86 insertions(+), 3 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 896ab6cc9f..de3cc73817 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1271,6 +1271,7 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() dataStream.seekp(-1, std::ios_base::end); dataStream << "},"; } + // pack the data for the FT Sensors if (m_streamFTSensors) { dataStream << "\"FTs\":{"; @@ -1283,7 +1284,92 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() dataStream << m_ftBuffer.coeff(i, 0) << ","; dataStream << m_ftBuffer.coeff(m_ftBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; dataStream << "\"elements_names\": [\"f_x\", \"f_y\", \"f_z\", \"mu_x\", \"mu_y\", \"mu_z\"]},"; + } + } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "},"; + } + // pack the data for the inertial measurements + if(m_streamInertials) + { + // pack the data for the gyros + dataStream << "\"gyros\":{ "; + for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) + { + dataStream << "\"" << sensorName << "\":{\"data\": ["; + if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) + { + for (unsigned int i = 0; i < m_gyroBuffer.rows(); i++) + dataStream << m_gyroBuffer.coeff(i, 0) << ","; + dataStream << m_gyroBuffer.coeff(m_gyroBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\": [\"r\", \"p\", \"y\"]},"; + } + } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "},"; + // pack the data for the accelerometer + dataStream << "\"accelerometers\":{ "; + for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) + { + if(m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) + { + dataStream << "\"" << sensorName << "\":{\"data\": ["; + for (unsigned int i = 0; i < m_acceloremeterBuffer.rows(); i++) + dataStream << m_acceloremeterBuffer.coeff(i, 0) << ","; + dataStream << m_acceloremeterBuffer.coeff(m_acceloremeterBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\": [\"a_x\", \"a_y\", \"a_z\"]},"; + } + } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "},"; + + + // pack the data for the orientations + dataStream << "\"orientations\":{ "; + for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) + { + if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) + { + dataStream << "\"" << sensorName << "\":{\"data\": ["; + for (unsigned int i = 0; i < m_orientationBuffer.rows(); i++) + dataStream << m_orientationBuffer.coeff(i, 0) << ","; + dataStream << m_orientationBuffer.coeff(m_orientationBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\": [\"r\", \"p\", \"y\"]},"; + + } + } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "},"; + + // pack the data for the magnemetometer + dataStream << "\"mangetometers\":{ "; + for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) + { + if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) + { + dataStream << "\"" << sensorName << "\":{\"data\": ["; + for (unsigned int i = 0; i < m_magnemetometerBuffer.rows(); i++) + dataStream << m_magnemetometerBuffer.coeff(i, 0) << ","; + dataStream << m_magnemetometerBuffer.coeff(m_magnemetometerBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\": [\"mag_x\", \"mag_y\", \"mag_z\"]},"; + + } + } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "},"; + + } + // pack the data for the temperature + if (m_streamTemperatureSensors) + { + dataStream << "\"tempuratures\":{"; + for (const auto& sensorname : m_robotSensorBridge->getTemperatureSensorsList()) + { + if (m_robotSensorBridge->getTemperature(sensorname, m_ftTemperatureBuffer)) + { + dataStream << "\"" << sensorname << "\":{\"data\": [" << m_ftTemperatureBuffer << "],"; + dataStream << "\"timestamps\": [" << time << "],\"elements_names\": [\"temperature\"]},"; } } dataStream.seekp(-1, std::ios_base::end); @@ -1400,11 +1486,8 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { - //dataStream << "gyros:: " << sensorName << ": " << m_gyroBuffer << "|"; m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); } - //out.addString(dataStream.str()); - //outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) From 98b056343d24cdc3330cc3f967895eeeaece80d0 Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 4 Jan 2024 10:00:45 +0100 Subject: [PATCH 08/83] Added Cartesian Wrench valus to be streamed in real-time --- .../src/YarpRobotLoggerDevice.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index de3cc73817..47c48a2439 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1375,6 +1375,25 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() dataStream.seekp(-1, std::ios_base::end); dataStream << "},"; } + + // pack the data for the cartesian wrenches + if (m_streamCartesianWrenches) + { + dataStream << "\"cartesian_wrenches\":{ "; + for (const auto& sensorName : m_robotSensorBridge->getCartesianWrenchesList()) + { + if (m_robotSensorBridge->getCartesianWrench(sensorName, m_ftBuffer)) + { + dataStream << "\"" << sensorName << "\":{\"data\": ["; + for (unsigned int i = 0; i < m_ftBuffer.rows() - 1; i++) + dataStream << m_ftBuffer.coeff(i, 0) << ","; + dataStream << m_ftBuffer.coeff(m_ftBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; + dataStream << "\"elements_names\": [\"f_x\", \"f_y\", \"f_z\", \"mu_x\", \"mu_y\", \"mu_z\"]},"; + } + } + dataStream.seekp(-1, std::ios_base::end); + dataStream << "},"; + } dataStream.seekp(-1, std::ios_base::end); dataStream << "}}"; std::cout << std::endl; From d498c21b7e4fc5935904bf1647c5de36b85d28b2 Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 4 Jan 2024 16:49:29 +0100 Subject: [PATCH 09/83] Added external data, moved current system of sending data to a better json format --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 5 + .../src/YarpRobotLoggerDevice.cpp | 348 ++++++++++++------ 2 files changed, 249 insertions(+), 104 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 20c6c0bfbd..93b143766c 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -186,6 +187,10 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, // for realtime data streaming void SendDataToLoggerVisualizer(); + + void PackFlightData(std::vector keyTokens, const std::vector& values, Json::Value& jsonValueToPack, const double& time); + + std::vector tokenizeSubString(std::string input, std::string delimiter); }; } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 47c48a2439..59f2ad1b11 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -66,6 +67,10 @@ VISITABLE_STRUCT(TextLoggingEntry, yarprun_timestamp, local_timestamp); +// TODO: REMOVE THIS GLOBAL VARIABLE, THIS IS JUST HERE FOR TESTING +BipedalLocomotion::YarpUtilities::VectorsCollection* collection; + + void findAndReplaceAll(std::string& data, const std::string& toSearch, const std::string& replaceStr) { // Get the first occurrence @@ -80,6 +85,7 @@ void findAndReplaceAll(std::string& data, const std::string& toSearch, const std } } + YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, yarp::os::ShouldUseSystemClock useSystemClock) : yarp::os::PeriodicThread(period, useSystemClock) @@ -1213,197 +1219,324 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() yarp::os::Bottle& realtimeOutput = realtimeLoggingPort.prepare(); realtimeOutput.clear(); std::stringstream dataStream; - dataStream << std::fixed << "{\"robot_realtime\":{"; + Json::Value root; + Json::Value& robotRealtime = root["robot_realtime"]; if (m_streamJointStates) { - bool dictionaryInitialized = false; + Json::Value& jointStates = robotRealtime["joint_states"]; std::vector joints; m_robotSensorBridge->getJointsList(joints); - dataStream << "\"joints_state\":{ "; - // pack the data for the joint positions + // prepare the json format for the joint positions if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) { - dataStream << "\"positions\":{\"data\":["; - for (unsigned int i = 0; i < m_jointSensorBuffer.rows() - 1; i++) - dataStream << m_jointSensorBuffer.coeff(i, 0) << ","; - dataStream << m_jointSensorBuffer.coeff(m_jointSensorBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\":["; - for(unsigned int i = 0; i < joints.size() - 1; i++) - dataStream << "\"" << joints[i] << "\","; - dataStream << "\"" << joints[joints.size() - 1] << "\"]},"; + Json::Value& jointPositions = jointStates["positions"]; + Json::Value& jointPositionData = jointPositions["data"]; + for (unsigned int i = 0; i < m_jointSensorBuffer.rows(); i++) + jointPositionData.append(m_jointSensorBuffer.coeff(i,0)); + + Json::Value& jointPositionElementNames = jointPositions["elements_names"]; + for(unsigned int i = 0; i < joints.size(); i++) + jointPositionElementNames.append(joints[i]); + + Json::Value& jointPositionTimeStamp = jointPositions["timestamps"]; + jointPositionTimeStamp.append(time); } + + // prepare the json format for the joint velocities if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) { - dataStream << "\"velocities\":{\"data\":["; - for (unsigned int i = 0; i < m_jointSensorBuffer.rows() - 1; i++) - dataStream << m_jointSensorBuffer.coeff(i, 0) << ","; - dataStream << m_jointSensorBuffer.coeff(m_jointSensorBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\":["; - for(unsigned int i = 0; i < joints.size() - 1; i++) - dataStream << "\"" << joints[i] << "\","; - dataStream << "\"" << joints[joints.size() - 1] << "\"]},"; + Json::Value& jointVelocities = jointStates["velocities"]; + Json::Value& jointVelocityData = jointVelocities["data"]; + for (unsigned int i = 0; i < m_jointSensorBuffer.rows(); i++) + jointVelocityData.append(m_jointSensorBuffer.coeff(i,0)); + + Json::Value& jointVelocityElementNames = jointVelocities["elements_names"]; + for(unsigned int i = 0; i < joints.size(); i++) + jointVelocityElementNames.append(joints[i]); + + Json::Value& jointVelocityTimeStamp = jointVelocities["timestamps"]; + jointVelocityTimeStamp.append(time); } + // prepare the json format for the joint accelerations if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) { - dataStream << "\"accelerations\":{\"data\":["; - for (unsigned int i = 0; i < m_jointSensorBuffer.rows() - 1; i++) - dataStream << m_jointSensorBuffer.coeff(i, 0) << ","; - dataStream << m_jointSensorBuffer.coeff(m_jointSensorBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\":["; - for(unsigned int i = 0; i < joints.size() - 1; i++) - dataStream << "\"" << joints[i] << "\","; - dataStream << "\"" << joints[joints.size() - 1] << "\"]},"; + Json::Value& jointAccelerations = jointStates["accelerations"]; + Json::Value& jointAccelerationData = jointAccelerations["data"]; + for (unsigned int i = 0; i < m_jointSensorBuffer.rows(); i++) + jointAccelerationData.append(m_jointSensorBuffer.coeff(i,0)); + + Json::Value& jointAccelerationElementNames = jointAccelerations["elements_names"]; + for(unsigned int i = 0; i < joints.size(); i++) + jointAccelerationElementNames.append(joints[i]); + + Json::Value& jointAccelerationTimeStamp = jointAccelerations["timestamps"]; + jointAccelerationTimeStamp.append(time); } + // prepare the json format for the joint torques if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { - dataStream << "\"torques\":{\"data\":["; - for (unsigned int i = 0; i < m_jointSensorBuffer.rows() - 1; i++) - dataStream << m_jointSensorBuffer.coeff(i, 0) << ","; - dataStream << m_jointSensorBuffer.coeff(m_jointSensorBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\":["; - for(unsigned int i = 0; i < joints.size() - 1; i++) - dataStream << "\"" << joints[i] << "\","; - dataStream << "\"" << joints[joints.size() - 1] << "\"]},"; + Json::Value& jointTorques = jointStates["torques"]; + Json::Value& jointTorqueData = jointTorques["data"]; + for (unsigned int i = 0; i < m_jointSensorBuffer.rows(); i++) + jointTorqueData.append(m_jointSensorBuffer.coeff(i,0)); + + Json::Value& jointTorqueElementNames = jointTorques["elements_names"]; + for(unsigned int i = 0; i < joints.size(); i++) + jointTorqueElementNames.append(joints[i]); + Json::Value& jointTorqueTimeStamp = jointTorques["timestamps"]; + jointTorqueTimeStamp.append(time); } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "},"; } // pack the data for the FT Sensors if (m_streamFTSensors) { - dataStream << "\"FTs\":{"; + Json::Value& FTSensors = robotRealtime["FTs"]; + std::string ftElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { - dataStream << "\"" << sensorName << "\":{\"data\": ["; - for (unsigned int i = 0; i < m_ftBuffer.rows() - 1; i++) - dataStream << m_ftBuffer.coeff(i, 0) << ","; - dataStream << m_ftBuffer.coeff(m_ftBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\": [\"f_x\", \"f_y\", \"f_z\", \"mu_x\", \"mu_y\", \"mu_z\"]},"; + Json::Value& FTSensor = FTSensors[sensorName]; + Json::Value& FTSensorData = FTSensor["data"]; + for (unsigned int i = 0; i < m_ftBuffer.rows(); i++) + FTSensorData.append(m_ftBuffer.coeff(i, 0)); + + Json::Value& FTSensorElementNames = FTSensor["elements_names"]; + for (std::string ftElementName : ftElementNames) + FTSensorElementNames.append(ftElementName); + Json::Value& FTSensorTimeStamp = FTSensor["timestamps"]; + FTSensorTimeStamp.append(time); } } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "},"; } + // pack the data for the inertial measurements if(m_streamInertials) { // pack the data for the gyros - dataStream << "\"gyros\":{ "; + Json::Value& gyros = robotRealtime["gyros"]; + std::string gyroElementNames[] = {"omega_x", "omega_y", "omega_z"}; for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { - dataStream << "\"" << sensorName << "\":{\"data\": ["; + Json::Value& gyroSensor = gyros[sensorName]; + Json::Value& gyroSensorData = gyroSensor["data"]; if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { for (unsigned int i = 0; i < m_gyroBuffer.rows(); i++) - dataStream << m_gyroBuffer.coeff(i, 0) << ","; - dataStream << m_gyroBuffer.coeff(m_gyroBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\": [\"r\", \"p\", \"y\"]},"; + gyroSensorData.append(m_gyroBuffer.coeff(i,0)); + + Json::Value& gyroSensorElementNames = gyroSensor["elements_names"]; + for(std::string gyroElementName : gyroElementNames) + gyroSensorElementNames.append(gyroElementName); + + Json::Value& gyroSensorTimeStamp = gyroSensor["timestamps"]; + gyroSensorTimeStamp.append(time); } } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "},"; // pack the data for the accelerometer - dataStream << "\"accelerometers\":{ "; + Json::Value& accelerometers = robotRealtime["accelerometers"]; + std::string accelerometerElementNames[] = {"a_x", "a_y", "a_z"}; for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { + Json::Value& accelerometerSensor = accelerometers[sensorName]; + Json::Value& accelerometerSensorData = accelerometerSensor["data"]; if(m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) { - dataStream << "\"" << sensorName << "\":{\"data\": ["; for (unsigned int i = 0; i < m_acceloremeterBuffer.rows(); i++) - dataStream << m_acceloremeterBuffer.coeff(i, 0) << ","; - dataStream << m_acceloremeterBuffer.coeff(m_acceloremeterBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\": [\"a_x\", \"a_y\", \"a_z\"]},"; + accelerometerSensorData.append(m_acceloremeterBuffer.coeff(i,0)); + + Json::Value& accelerometerSensorElementNames = accelerometerSensor["elements_names"]; + for(std::string accelerometerElementName : accelerometerElementNames) + accelerometerSensorElementNames.append(accelerometerElementName); + + Json::Value& accelerometerSensorTimeStamp = accelerometerSensor["timestamps"]; + accelerometerSensorTimeStamp.append(time); } } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "},"; - // pack the data for the orientations - dataStream << "\"orientations\":{ "; + Json::Value& orientations = robotRealtime["orientations"]; + std::string orientationElementNames[] = {"r", "p", "y"}; for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { + Json::Value& orientationSensor = orientations[sensorName]; + Json::Value& orientationSensorData = orientationSensor["data"]; if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) { - dataStream << "\"" << sensorName << "\":{\"data\": ["; for (unsigned int i = 0; i < m_orientationBuffer.rows(); i++) - dataStream << m_orientationBuffer.coeff(i, 0) << ","; - dataStream << m_orientationBuffer.coeff(m_orientationBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\": [\"r\", \"p\", \"y\"]},"; - + orientationSensorData.append(m_orientationBuffer.coeff(i,0)); + + Json::Value& orientationSensorElementNames = orientationSensor["elements_names"]; + for(std::string orientationElementName : orientationElementNames) + orientationSensorElementNames.append(orientationElementName); + + Json::Value& orientationSensorTimeStamp = orientationSensor["timestamps"]; + orientationSensorTimeStamp.append(time); } } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "},"; // pack the data for the magnemetometer - dataStream << "\"mangetometers\":{ "; + Json::Value& mangetometers = robotRealtime["mangetometers"]; + std::string mangetometerElementNames[] = {"mag_x", "mag_y", "mag_z"}; for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { + Json::Value& mangetometerSensor = mangetometers[sensorName]; + Json::Value& mangetometerSensorData = mangetometerSensor["data"]; if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { - dataStream << "\"" << sensorName << "\":{\"data\": ["; for (unsigned int i = 0; i < m_magnemetometerBuffer.rows(); i++) - dataStream << m_magnemetometerBuffer.coeff(i, 0) << ","; - dataStream << m_magnemetometerBuffer.coeff(m_magnemetometerBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\": [\"mag_x\", \"mag_y\", \"mag_z\"]},"; - + mangetometerSensorData.append(m_magnemetometerBuffer.coeff(i,0)); + + Json::Value& mangetomterSensorElementNames = mangetometerSensor["elements_names"]; + for(std::string mangetometerElementName : mangetometerElementNames) + mangetomterSensorElementNames.append(mangetometerElementName); + + Json::Value& mangetometerSensorTimeStamp = mangetometerSensor["timestamps"]; + mangetometerSensorTimeStamp.append(time); } } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "},"; - } // pack the data for the temperature if (m_streamTemperatureSensors) { - dataStream << "\"tempuratures\":{"; - for (const auto& sensorname : m_robotSensorBridge->getTemperatureSensorsList()) + Json::Value& temperatures = robotRealtime["temperatures"]; + for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { - if (m_robotSensorBridge->getTemperature(sensorname, m_ftTemperatureBuffer)) + Json::Value& temperatureSensor = temperatures[sensorName]; + Json::Value& temperatureSensorData = temperatureSensor["data"]; + if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) { - dataStream << "\"" << sensorname << "\":{\"data\": [" << m_ftTemperatureBuffer << "],"; - dataStream << "\"timestamps\": [" << time << "],\"elements_names\": [\"temperature\"]},"; + temperatureSensorData.append(m_ftTemperatureBuffer); + + Json::Value& temperatureSensorElementNames = temperatureSensor["elements_names"]; + temperatureSensorElementNames.append("temperature"); + + Json::Value& temperatureSensorTimeStamp = temperatureSensor["timestamps"]; + temperatureSensorTimeStamp.append(time); } } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "},"; } // pack the data for the cartesian wrenches if (m_streamCartesianWrenches) { - dataStream << "\"cartesian_wrenches\":{ "; - for (const auto& sensorName : m_robotSensorBridge->getCartesianWrenchesList()) + Json::Value& cartesianWrenches = robotRealtime["cartesian_wrenches"]; + std::string cartesianElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; + for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { - if (m_robotSensorBridge->getCartesianWrench(sensorName, m_ftBuffer)) + Json::Value& cartesianWrench = cartesianWrenches[cartesianWrenchName]; + Json::Value& cartesianWrenchData = cartesianWrench["data"]; + if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) { - dataStream << "\"" << sensorName << "\":{\"data\": ["; - for (unsigned int i = 0; i < m_ftBuffer.rows() - 1; i++) - dataStream << m_ftBuffer.coeff(i, 0) << ","; - dataStream << m_ftBuffer.coeff(m_ftBuffer.rows() - 1, 0) << "], \"timestamps\": [" << time << "],"; - dataStream << "\"elements_names\": [\"f_x\", \"f_y\", \"f_z\", \"mu_x\", \"mu_y\", \"mu_z\"]},"; + for (unsigned int i = 0; i < m_ftBuffer.rows(); i++) + cartesianWrenchData.append(m_ftBuffer(i, 0)); + + Json::Value& cartesianWrenchElementNames = cartesianWrench["elements_names"]; + for(std::string cartesianElementName : cartesianElementNames) + cartesianWrenchElementNames.append(cartesianElementName); + + Json::Value& cartesianWrenchTimeStamp = cartesianWrench["timestamps"]; + cartesianWrenchTimeStamp.append(time); + } } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "},"; } - dataStream.seekp(-1, std::ios_base::end); - dataStream << "}}"; + + // pack the external data + Json::Value& flightData = robotRealtime["FlightData"]; + for (auto& [name, signal] : m_vectorsCollectionSignals) + { + std::lock_guard lock(signal.mutex); + // collection is populated from the global variable + if (collection != nullptr) + { + for (const auto& [key, vector] : collection->vectors) + { + std::vector keyTokens = tokenizeSubString(key, "::"); + PackFlightData(keyTokens, vector, flightData, time); + + + + // std::string jsonKeyTree = std::regex_replace(key, std::regex("ExternalData::"), ""); + // jsonKeyTree = std::regex_replace(jsonKeyTree, std::regex("::"), "\":{\""); + // // std::cout << "about to add a channel for external signal: " << signalFullName << std::endl; + // dataStream << jsonKeyTree << "\": { \"data\": ["; + // for(unsigned int i = 0; i < vector.size() - 1; i++) + // dataStream << vector[i] << ","; + // dataStream << vector[vector.size() - 1] << "], \"timestamps\": [" << time << "]},"; + } + // for (const auto& [key, vector] : collection->vectors) + // { + // signalFullName = signal.signalName + "::" + key; + // std::cout << "About to push back data for: " << signalFullName << std::endl; + // m_bufferManager.push_back(vector, time, signalFullName); + // std::cout << "Added the data for: " << signalFullName << std::endl; + // std::cout << std::endl; + // } + } + } + Json::FastWriter fastWriter; + std::string jsonDataToSend = fastWriter.write(root); + std::cout << std::endl; - std::cout << dataStream.str() << std::endl; + std::cout << jsonDataToSend << std::endl; std::cout << std::endl; - realtimeOutput.addString(dataStream.str()); + + realtimeOutput.addString(jsonDataToSend); realtimeLoggingPort.write(true); } +void YarpRobotLoggerDevice::PackFlightData(std::vector keyTokens, const std::vector& values, Json::Value& jsonValueToPack, const double& time) +{ + std::cout << "In Pack Flight Data function" << std::endl; + if(keyTokens.size() == 1) + { + std::cout << "Final Key: " << keyTokens[0] << std::endl; + Json::Value& jsonFinalValueToPack = jsonValueToPack[keyTokens[0]]; + Json::Value& jsonFinalValueData = jsonFinalValueToPack["data"]; + for(unsigned int i = 0; i < values.size(); i++) + jsonFinalValueData.append(values[i]); + + Json::Value& jsonFinalValueTimeStamp = jsonFinalValueToPack["timestamps"]; + jsonFinalValueTimeStamp.append(time); + + return; + } + + //for(unsigned int i = 0; i < keyTokens.size(); i++) + //{ + Json::Value& subValuesToPack = jsonValueToPack[keyTokens[0]]; + std::vector subTokens = {keyTokens.begin() + 1, keyTokens.end()}; + std::cout << "Sub Vector Values: {"; + for (unsigned int i = 0; i < subTokens.size(); i++) + std::cout << subTokens[i] << ", "; + std::cout << "}, " << keyTokens.size() << ":" << subTokens.size() << std::endl; + //} + PackFlightData(subTokens, values, subValuesToPack, time); +} + +std::vector YarpRobotLoggerDevice::tokenizeSubString(std::string input, std::string delimiter) +{ + size_t position = 0; + std::string token; + std::vector tokens; + while ((position = input.find(delimiter)) != std::string::npos) + { + token = input.substr(0, position); + std::cout << token << std::endl; + tokens.push_back(token); + input.erase(0, position + delimiter.length()); + } + tokens.push_back(input); + + return tokens; +} + void YarpRobotLoggerDevice::run() { constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; @@ -1583,7 +1716,7 @@ void YarpRobotLoggerDevice::run() for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); - BipedalLocomotion::YarpUtilities::VectorsCollection* collection = signal.port.read(false); + collection = signal.port.read(false); if (collection != nullptr) { if (!signal.dataArrived) @@ -1591,20 +1724,26 @@ void YarpRobotLoggerDevice::run() for (const auto& [key, vector] : collection->vectors) { signalFullName = signal.signalName + "::" + key; + // std::cout << "about to add a channel for xternal signal: " << signalFullName << std::endl; m_bufferManager.addChannel({signalFullName, {vector.size(), 1}}); + // std::cout << "Added a channel for external signal: " << signalFullName << std::endl; + std::cout << std::endl; } signal.dataArrived = true; } for (const auto& [key, vector] : collection->vectors) { -// signalFullName = signal.signalName + "::" + key; -// dataStream << "signalFullName: " << vector << "|"; + signalFullName = signal.signalName + "::" + key; + // std::cout << "About to push back data for: " << signalFullName << std::endl; + // for(unsigned int i = 0; i < vector.size(); i++) + // std::cout << vector[i] << ","; + // std::cout << std::endl << std::endl;; m_bufferManager.push_back(vector, time, signalFullName); + // std::cout << "Added the data for: " << signalFullName << std::endl; + std::cout << std::endl; } } -// out.addString(dataStream.str()); -// outPort.write(true); } for (auto& [name, signal] : m_vectorSignals) @@ -1661,6 +1800,7 @@ void YarpRobotLoggerDevice::run() break; } } + SendDataToLoggerVisualizer(); } From cf20448ec46e8dc0292ab2ac6e2279f90abc61d3 Mon Sep 17 00:00:00 2001 From: Nick Date: Fri, 5 Jan 2024 12:28:46 +0100 Subject: [PATCH 10/83] Fixed bug related to json key field --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 59f2ad1b11..1dbc4634ac 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1223,7 +1223,7 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() Json::Value& robotRealtime = root["robot_realtime"]; if (m_streamJointStates) { - Json::Value& jointStates = robotRealtime["joint_states"]; + Json::Value& jointStates = robotRealtime["joints_state"]; std::vector joints; m_robotSensorBridge->getJointsList(joints); // prepare the json format for the joint positions From 296cd2cfac5c61e8a328b439b1a76326cb625da5 Mon Sep 17 00:00:00 2001 From: Nick Date: Fri, 5 Jan 2024 12:29:04 +0100 Subject: [PATCH 11/83] Cleaned up the code a little bit --- .../src/YarpRobotLoggerDevice.cpp | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 1dbc4634ac..66ea38d5c3 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1458,25 +1458,8 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() { std::vector keyTokens = tokenizeSubString(key, "::"); PackFlightData(keyTokens, vector, flightData, time); - - - - // std::string jsonKeyTree = std::regex_replace(key, std::regex("ExternalData::"), ""); - // jsonKeyTree = std::regex_replace(jsonKeyTree, std::regex("::"), "\":{\""); - // // std::cout << "about to add a channel for external signal: " << signalFullName << std::endl; - // dataStream << jsonKeyTree << "\": { \"data\": ["; - // for(unsigned int i = 0; i < vector.size() - 1; i++) - // dataStream << vector[i] << ","; - // dataStream << vector[vector.size() - 1] << "], \"timestamps\": [" << time << "]},"; } - // for (const auto& [key, vector] : collection->vectors) - // { - // signalFullName = signal.signalName + "::" + key; - // std::cout << "About to push back data for: " << signalFullName << std::endl; - // m_bufferManager.push_back(vector, time, signalFullName); - // std::cout << "Added the data for: " << signalFullName << std::endl; - // std::cout << std::endl; - // } + } } Json::FastWriter fastWriter; From a735dd4b4a19266bbd2959033ac97f1c51203615 Mon Sep 17 00:00:00 2001 From: Nick Date: Fri, 5 Jan 2024 15:12:35 +0100 Subject: [PATCH 12/83] Cleaned up the code, removed hard-coded values --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 1 + .../src/YarpRobotLoggerDevice.cpp | 118 +++--------------- 2 files changed, 18 insertions(+), 101 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 93b143766c..df2bac30db 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -161,6 +161,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, // for realtime data streaming bool streamRealTime{true}; yarp::os::BufferedPort realtimeLoggingPort; + BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection; std::mutex m_bufferManagerMutex; robometry::BufferManager m_bufferManager; diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 66ea38d5c3..a482ef2b0d 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -68,7 +68,6 @@ VISITABLE_STRUCT(TextLoggingEntry, local_timestamp); // TODO: REMOVE THIS GLOBAL VARIABLE, THIS IS JUST HERE FOR TESTING -BipedalLocomotion::YarpUtilities::VectorsCollection* collection; void findAndReplaceAll(std::string& data, const std::string& toSearch, const std::string& replaceStr) @@ -1218,9 +1217,12 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() yarp::os::Bottle& realtimeOutput = realtimeLoggingPort.prepare(); realtimeOutput.clear(); - std::stringstream dataStream; Json::Value root; Json::Value& robotRealtime = root["robot_realtime"]; + Json::Value& descriptionList = robotRealtime["description_list"]; + Json::Value& yarpRobotName = robotRealtime["yarp_robot_name"]; + char* tmp = std::getenv("YARP_ROBOT_NAME"); + yarpRobotName.append(std::string(tmp)); if (m_streamJointStates) { Json::Value& jointStates = robotRealtime["joints_state"]; @@ -1236,7 +1238,10 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() Json::Value& jointPositionElementNames = jointPositions["elements_names"]; for(unsigned int i = 0; i < joints.size(); i++) + { + descriptionList.append(joints[i]); jointPositionElementNames.append(joints[i]); + } Json::Value& jointPositionTimeStamp = jointPositions["timestamps"]; jointPositionTimeStamp.append(time); @@ -1452,23 +1457,18 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() { std::lock_guard lock(signal.mutex); // collection is populated from the global variable - if (collection != nullptr) + if (externalSignalCollection != nullptr) { - for (const auto& [key, vector] : collection->vectors) + for (const auto& [key, vector] : externalSignalCollection->vectors) { std::vector keyTokens = tokenizeSubString(key, "::"); PackFlightData(keyTokens, vector, flightData, time); } - } } Json::FastWriter fastWriter; std::string jsonDataToSend = fastWriter.write(root); - std::cout << std::endl; - std::cout << jsonDataToSend << std::endl; - std::cout << std::endl; - realtimeOutput.addString(jsonDataToSend); realtimeLoggingPort.write(true); @@ -1476,10 +1476,8 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() void YarpRobotLoggerDevice::PackFlightData(std::vector keyTokens, const std::vector& values, Json::Value& jsonValueToPack, const double& time) { - std::cout << "In Pack Flight Data function" << std::endl; if(keyTokens.size() == 1) { - std::cout << "Final Key: " << keyTokens[0] << std::endl; Json::Value& jsonFinalValueToPack = jsonValueToPack[keyTokens[0]]; Json::Value& jsonFinalValueData = jsonFinalValueToPack["data"]; for(unsigned int i = 0; i < values.size(); i++) @@ -1491,15 +1489,9 @@ void YarpRobotLoggerDevice::PackFlightData(std::vector keyTokens, c return; } - //for(unsigned int i = 0; i < keyTokens.size(); i++) - //{ - Json::Value& subValuesToPack = jsonValueToPack[keyTokens[0]]; - std::vector subTokens = {keyTokens.begin() + 1, keyTokens.end()}; - std::cout << "Sub Vector Values: {"; - for (unsigned int i = 0; i < subTokens.size(); i++) - std::cout << subTokens[i] << ", "; - std::cout << "}, " << keyTokens.size() << ":" << subTokens.size() << std::endl; - //} + + Json::Value& subValuesToPack = jsonValueToPack[keyTokens[0]]; + std::vector subTokens = {keyTokens.begin() + 1, keyTokens.end()}; PackFlightData(subTokens, values, subValuesToPack, time); } @@ -1511,7 +1503,6 @@ std::vector YarpRobotLoggerDevice::tokenizeSubString(std::string in while ((position = input.find(delimiter)) != std::string::npos) { token = input.substr(0, position); - std::cout << token << std::endl; tokens.push_back(token); input.erase(0, position + delimiter.length()); } @@ -1550,114 +1541,64 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { - // dataStream << "Motor State Positions: " << m_jointSensorBuffer << "|"; } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { - // dataStream << "Motor State Velocities: " << m_jointSensorBuffer << "|"; } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { - // dataStream << "Motor State Accelerations: " << m_jointSensorBuffer << "|"; } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { - // dataStream << "Motor State Currents: " << m_jointSensorBuffer << "|"; } - //out.addString(dataStream.str()); - //outPort.write(true); } if (m_streamMotorPWM) { if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) - { - //dataStream << "Motor State PWMs: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::PWM"); - } - //out.addString(dataStream.str()); - //outPort.write(true); } if (m_streamPIDs) { if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) - { - //dataStream << "PIDs: " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_jointSensorBuffer, time, "PIDs"); - } - // out.addString(dataStream.str()); - // outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) - { m_bufferManager.push_back(m_ftBuffer, time, "FTs::" + sensorName); - } } - // dataStream.seekp(-1, std::ios_base::end); - // dataStream << "}}}"; - // out.addString(dataStream.str()); - // outPort.write(true); - // std::cout << std::endl; - // std::cout << dataStream.str() << std::endl; - // std::cout << std::endl; - // dataStream.str(std::string()); for (const auto& sensorname : m_robotSensorBridge->getTemperatureSensorsList()) { if (m_robotSensorBridge->getTemperature(sensorname, m_ftTemperatureBuffer)) - { - //dataStream << "temperatures:: " << sensorname << ": " << m_ftTemperatureBuffer << "|"; m_bufferManager.push_back({m_ftTemperatureBuffer}, time, "temperatures::" + sensorname); - } - //out.addString(dataStream.str()); - //outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) - { m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); - } } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { - if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, - m_acceloremeterBuffer)) - { - //dataStream << "accelerometers:: " << sensorName << ": " << m_acceloremeterBuffer << "|"; + if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); - } - //out.addString(dataStream.str()); - //outPort.write(true); - } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) - { - //dataStream << "orientations:: " << sensorName << ": " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); - } - //out.addString(dataStream.str()); - //outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) - { - //dataStream << "magnetometers:: " << sensorName << ": " << m_jointSensorBuffer << "|"; m_bufferManager.push_back(m_magnemetometerBuffer, time, "magnetometers::" + sensorName); - } - //out.addString(dataStream.str()); - //outPort.write(true); } // an IMU contains a gyro accelerometer and an orientation sensor @@ -1671,60 +1612,43 @@ void YarpRobotLoggerDevice::run() m_gyroBuffer, m_orientationBuffer); - //dataStream << "accelerometers::" << sensorName << ": " << m_acceloremeterBuffer << "|"; m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); - //dataStream << "gyros::" << sensorName << ": " << m_gyroBuffer << "|"; m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); - //dataStream << "orientations::" << sensorName << ": " << m_orientationBuffer << "|"; m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); } - //out.addString(dataStream.str()); - //outPort.write(true); } for (const auto& sensorName : m_robotSensorBridge->getCartesianWrenchesList()) { if (m_robotSensorBridge->getCartesianWrench(sensorName, m_ftBuffer)) { - //dataStream << "caresian_wrenches::" << sensorName << ": " << m_ftBuffer << "|"; m_bufferManager.push_back(m_ftBuffer, time, "cartesian_wrenches::" + sensorName); } - //out.addString(dataStream.str()); - //outPort.write(true); } std::string signalFullName; for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); - collection = signal.port.read(false); - if (collection != nullptr) + externalSignalCollection = signal.port.read(false); + if (externalSignalCollection != nullptr) { if (!signal.dataArrived) { - for (const auto& [key, vector] : collection->vectors) + for (const auto& [key, vector] : externalSignalCollection->vectors) { signalFullName = signal.signalName + "::" + key; - // std::cout << "about to add a channel for xternal signal: " << signalFullName << std::endl; m_bufferManager.addChannel({signalFullName, {vector.size(), 1}}); - // std::cout << "Added a channel for external signal: " << signalFullName << std::endl; - std::cout << std::endl; } signal.dataArrived = true; } - for (const auto& [key, vector] : collection->vectors) + for (const auto& [key, vector] : externalSignalCollection->vectors) { signalFullName = signal.signalName + "::" + key; - // std::cout << "About to push back data for: " << signalFullName << std::endl; - // for(unsigned int i = 0; i < vector.size(); i++) - // std::cout << vector[i] << ","; - // std::cout << std::endl << std::endl;; m_bufferManager.push_back(vector, time, signalFullName); - // std::cout << "Added the data for: " << signalFullName << std::endl; - std::cout << std::endl; } } } @@ -1740,11 +1664,7 @@ void YarpRobotLoggerDevice::run() m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); signal.dataArrived = true; } -// dataStream << signal.signalName << ": " << *vector << "|"; -// m_bufferManager.push_back(*vector, time, signal.signalName); } -// out.addString(dataStream.str()); -// outPort.write(true); } int bufferportSize = m_textLoggingPort.getPendingReads(); @@ -1772,11 +1692,7 @@ void YarpRobotLoggerDevice::run() m_textLogsStoredInManager.insert(signalFullName); } -// dataStream << signalFullName << ": " << msg << "|"; -// m_bufferManager.push_back(msg, time, signalFullName); } -// out.addString(dataStream.str()); -// outPort.write(true); bufferportSize = m_textLoggingPort.getPendingReads(); } else { From f43d2d74cff3ff91e149cb8dbb8c8580db5149b0 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 9 Jan 2024 09:57:05 +0100 Subject: [PATCH 13/83] Added back motor state logging --- .../src/YarpRobotLoggerDevice.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index a482ef2b0d..31dad4b885 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1294,6 +1294,10 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() Json::Value& jointTorqueTimeStamp = jointTorques["timestamps"]; jointTorqueTimeStamp.append(time); } + } + if (m_streamMotorStates) + { + } // pack the data for the FT Sensors if (m_streamFTSensors) @@ -1540,17 +1544,13 @@ void YarpRobotLoggerDevice::run() if (m_streamMotorStates) { if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) - { - } + m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::positions"); if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) - { - } + m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::velocities"); if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) - { - } + m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::accelerations"); if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) - { - } + m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::currents"); } if (m_streamMotorPWM) From 325d378b90e20343a15a5dc138ede344ed62e99d Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 9 Jan 2024 10:51:05 +0100 Subject: [PATCH 14/83] Changed rt logging port name --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 31dad4b885..14b0886bed 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -99,7 +99,7 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, // for realtime logging if (streamRealTime) - realtimeLoggingPort.open("/testLoggerOutput"); + realtimeLoggingPort.open("/YARPRobotLoggerRT:o"); } YarpRobotLoggerDevice::YarpRobotLoggerDevice() @@ -114,7 +114,7 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() std::make_shared()); if (streamRealTime) - realtimeLoggingPort.open("/testLoggerOutput"); + realtimeLoggingPort.open("/YARPRobotLoggerRT:o"); } YarpRobotLoggerDevice::~YarpRobotLoggerDevice() = default; From 5bd01224a3d99d757ec2eedec5fdb09ff2c69c27 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 15 Jan 2024 12:05:26 +0100 Subject: [PATCH 15/83] Added Vector Collection Client --- .../YarpUtilities/VectorsCollection.h | 1 + bindings/python/YarpUtilities/src/Module.cpp | 2 ++ .../YarpUtilities/src/VectorsCollection.cpp | 27 +++++++++++++++++++ .../BipedalLocomotion/YarpRobotLoggerDevice.h | 1 - 4 files changed, 30 insertions(+), 1 deletion(-) diff --git a/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h b/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h index 1c54f5ce2a..101508ef8b 100644 --- a/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h +++ b/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h @@ -18,6 +18,7 @@ namespace YarpUtilities { void CreateVectorsCollectionServer(pybind11::module& module); +void CreateVectorsCollectionClient(pybind11::module& module); } // namespace YarpUtilities } // namespace bindings diff --git a/bindings/python/YarpUtilities/src/Module.cpp b/bindings/python/YarpUtilities/src/Module.cpp index 660f20cb63..7f24495cce 100644 --- a/bindings/python/YarpUtilities/src/Module.cpp +++ b/bindings/python/YarpUtilities/src/Module.cpp @@ -20,6 +20,8 @@ void CreateModule(pybind11::module& module) module.doc() = "YarpUtilities module."; CreateVectorsCollectionServer(module); + CreateVectorsCollectionClient(module); + } } // namespace IK } // namespace bindings diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index d1d95bb458..e24ce36118 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -10,6 +10,7 @@ #include #include +#include #include #include @@ -45,6 +46,32 @@ void CreateVectorsCollectionServer(pybind11::module& module) return impl.populateData(key, data); }); } + +void CreateVectorsCollectionClient(pybind11::module& module) +{ + namespace py = ::pybind11; + + using namespace ::BipedalLocomotion::YarpUtilities; + + py::class_(module, "VectorsCollectionClient") + .def(py::init()) + .def( + "initialize", + [](VectorsCollectionClient& impl, + std::shared_ptr + handler) -> bool { return impl.initialize(handler); }, + py::arg("handler")) + .def("connect", &VectorsCollectionClient::connect) + .def("disconnect", &VectorsCollectionClient::disconnect) + .def("getMetadata", &VectorsCollectionClient::getMetadata, py::arg("force_strict") = false) + .def("readData", /*&VectorsCollectionClient::readData, py::arg("shouldWait") = true, + py::return_value_policy::reference_internal); */ + [](VectorsCollectionClient& impl, bool shouldWait) -> std::map> + { + BipedalLocomotion::YarpUtilities::VectorsCollection* collection = impl.readData(shouldWait); + return collection->vectors; + }); +} } // namespace YarpUtilities } // namespace bindings } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 39dccbfc0f..489b8b5d9b 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -199,7 +199,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool createFramesFolder(std::shared_ptr imageSaver, const std::string& camera, const std::string& imageType); - const std::string& camera, const std::string& imageType); // for realtime data streaming void SendDataToLoggerVisualizer(); From ca1fffb4dd2cbbdda7fb19a231f40af857bd22c3 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 15 Jan 2024 12:05:53 +0100 Subject: [PATCH 16/83] Initial test sucessful of sending vector collection data to robot-log-visualizer --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 6 + .../src/YarpRobotLoggerDevice.cpp | 243 ++++++++++-------- 2 files changed, 139 insertions(+), 110 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 489b8b5d9b..854cef2d8b 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -33,6 +33,7 @@ #include #include #include +#include namespace BipedalLocomotion { @@ -65,6 +66,8 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::unique_ptr m_robotSensorBridge; std::unique_ptr m_cameraBridge; + BipedalLocomotion::YarpUtilities::VectorsCollectionServer vectorCollectionRTDataServer; + template struct ExogenousSignal { std::mutex mutex; @@ -107,6 +110,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::unordered_map m_vectorsCollectionSignals; std::unordered_map> m_vectorSignals; + std::unordered_set m_exogenousPortsStoredInManager; std::atomic m_lookForNewExogenousSignalIsRunning{false}; std::thread m_lookForNewExogenousSignalThread; @@ -203,6 +207,8 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, // for realtime data streaming void SendDataToLoggerVisualizer(); + void ConfigureVectorCollectionServer(); + void PackFlightData(std::vector keyTokens, const std::vector& values, Json::Value& jsonValueToPack, const double& time); std::vector tokenizeSubString(std::string input, std::string delimiter); diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index d06089c43b..66cd581855 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -892,6 +893,10 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } } + std::cout << "Initializing vector collection server" << std::endl; + ConfigureVectorCollectionServer(); + std::cout << "Vector collection server has been initalized" << std::endl; + ok = ok && m_bufferManager.setSaveCallback( [this](const std::string& filePrefix, @@ -1252,90 +1257,106 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit } } -void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() +void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() { - const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); - - yarp::os::Bottle& realtimeOutput = realtimeLoggingPort.prepare(); - realtimeOutput.clear(); - Json::Value root; - Json::Value& robotRealtime = root["robot_realtime"]; - Json::Value& descriptionList = robotRealtime["description_list"]; - Json::Value& yarpRobotName = robotRealtime["yarp_robot_name"]; - char* tmp = std::getenv("YARP_ROBOT_NAME"); - yarpRobotName.append(std::string(tmp)); - if (m_streamJointStates) + auto loggerOption = std::make_shared(); + std::cout << "About to set the parameter for the vector collection server" << std::endl; + loggerOption->setParameter("remote", "/testVectorCollections"); + std::cout << "About to configure the Collection Server" << std::endl; + if (!vectorCollectionRTDataServer.initialize(loggerOption)) { - Json::Value& jointStates = robotRealtime["joints_state"]; - std::vector joints; - m_robotSensorBridge->getJointsList(joints); - // prepare the json format for the joint positions - if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) - { - Json::Value& jointPositions = jointStates["positions"]; - Json::Value& jointPositionData = jointPositions["data"]; - for (unsigned int i = 0; i < m_jointSensorBuffer.rows(); i++) - jointPositionData.append(m_jointSensorBuffer.coeff(i,0)); + std::cout << "Failed to initalize the vectorsCollectionServer" << std::endl; + return; + } + char* tmp = std::getenv("YARP_ROBOT_NAME"); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::yarp_robot_name", {std::string(tmp)}); - Json::Value& jointPositionElementNames = jointPositions["elements_names"]; - for(unsigned int i = 0; i < joints.size(); i++) - { - descriptionList.append(joints[i]); - jointPositionElementNames.append(joints[i]); - } + std::vector joints; + m_robotSensorBridge->getJointsList(joints); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::description_list", joints); + // confgure the metaData for the joints_state + if(m_streamJointStates) + { + vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::positions", joints); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::velocities", joints); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::accelerations", joints); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::torques", joints); + } - Json::Value& jointPositionTimeStamp = jointPositions["timestamps"]; - jointPositionTimeStamp.append(time); + // configure the metadata for the FTs + if(m_streamFTSensors) + { + for (const auto& sensorName: m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) + { + std::string fullFTSensorName = "robot_realtime::FTs::" + sensorName; + vectorCollectionRTDataServer.populateMetadata(fullFTSensorName, {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}); } + } - // prepare the json format for the joint velocities - if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) + // configure the metadata for the inertail measurments + if (m_streamInertials) + { + for (const auto& sensorName: m_robotSensorBridge->getGyroscopesList()) { - Json::Value& jointVelocities = jointStates["velocities"]; - Json::Value& jointVelocityData = jointVelocities["data"]; - for (unsigned int i = 0; i < m_jointSensorBuffer.rows(); i++) - jointVelocityData.append(m_jointSensorBuffer.coeff(i,0)); - - Json::Value& jointVelocityElementNames = jointVelocities["elements_names"]; - for(unsigned int i = 0; i < joints.size(); i++) - jointVelocityElementNames.append(joints[i]); - - Json::Value& jointVelocityTimeStamp = jointVelocities["timestamps"]; - jointVelocityTimeStamp.append(time); - + std::string fullGyroSensorName = "robot_realtime::gyros::" + sensorName; + vectorCollectionRTDataServer.populateMetadata(fullGyroSensorName, {"omega_x", "omega_y", "omega_z"}); } - // prepare the json format for the joint accelerations - if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) + for (const auto& sensorName: m_robotSensorBridge->getLinearAccelerometersList()) { - Json::Value& jointAccelerations = jointStates["accelerations"]; - Json::Value& jointAccelerationData = jointAccelerations["data"]; - for (unsigned int i = 0; i < m_jointSensorBuffer.rows(); i++) - jointAccelerationData.append(m_jointSensorBuffer.coeff(i,0)); - - Json::Value& jointAccelerationElementNames = jointAccelerations["elements_names"]; - for(unsigned int i = 0; i < joints.size(); i++) - jointAccelerationElementNames.append(joints[i]); - - Json::Value& jointAccelerationTimeStamp = jointAccelerations["timestamps"]; - jointAccelerationTimeStamp.append(time); - + std::string fullAccelerometerSensorName = "robot_realtime::accelerometers::" + sensorName; + vectorCollectionRTDataServer.populateMetadata(fullAccelerometerSensorName, {"a_x", "a_y", "a_z"}); } - // prepare the json format for the joint torques - if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) + for (const auto& sensorName: m_robotSensorBridge->getOrientationSensorsList()) + { + std::string fullOrientationSensorName = "robot_realtime::orientations::" + sensorName; + vectorCollectionRTDataServer.populateMetadata(fullOrientationSensorName, {"r", "p", "y"}); + } + for (const auto& sensorName: m_robotSensorBridge->getMagnetometersList()) { - Json::Value& jointTorques = jointStates["torques"]; - Json::Value& jointTorqueData = jointTorques["data"]; - for (unsigned int i = 0; i < m_jointSensorBuffer.rows(); i++) - jointTorqueData.append(m_jointSensorBuffer.coeff(i,0)); + std::string fullMagnetometersSensorName = "robot_realtime::magnetometers::" + sensorName; + vectorCollectionRTDataServer.populateMetadata(fullMagnetometersSensorName, {"mag_x", "mag_y", "mag_z"}); + } + } - Json::Value& jointTorqueElementNames = jointTorques["elements_names"]; - for(unsigned int i = 0; i < joints.size(); i++) - jointTorqueElementNames.append(joints[i]); + if(m_streamTemperatureSensors) + { + for (const auto& sensorName: m_robotSensorBridge->getTemperatureSensorsList()) + { + std::string fullTemperatureSensorName = "robot_realtime::temperatures::" + sensorName; + vectorCollectionRTDataServer.populateMetadata(fullTemperatureSensorName, {"temperature"}); + } + } - Json::Value& jointTorqueTimeStamp = jointTorques["timestamps"]; - jointTorqueTimeStamp.append(time); + if (m_streamCartesianWrenches) + { + for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) + { + std::string fullCartesianWrenchName = "robot_realtime::cartesian_wrenches::" + cartesianWrenchName; + vectorCollectionRTDataServer.populateMetadata(fullCartesianWrenchName, {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}); } } + + vectorCollectionRTDataServer.finalizeMetadata(); +} + +void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() +{ + const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); + + vectorCollectionRTDataServer.clearData(); + + if (m_streamJointStates) + { + if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) + vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::positions", m_jointSensorBuffer); + if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) + vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::velocities", m_jointSensorBuffer); + if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) + vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::accelerations", m_jointSensorBuffer); + if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) + vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::torques", m_jointSensorBuffer); + } + /* if (m_streamMotorStates) { @@ -1343,21 +1364,21 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() // pack the data for the FT Sensors if (m_streamFTSensors) { - Json::Value& FTSensors = robotRealtime["FTs"]; + // Json::Value& FTSensors = robotRealtime["FTs"]; std::string ftElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { - Json::Value& FTSensor = FTSensors[sensorName]; - Json::Value& FTSensorData = FTSensor["data"]; + // Json::Value& FTSensor = FTSensors[sensorName]; + // Json::Value& FTSensorData = FTSensor["data"]; for (unsigned int i = 0; i < m_ftBuffer.rows(); i++) FTSensorData.append(m_ftBuffer.coeff(i, 0)); - Json::Value& FTSensorElementNames = FTSensor["elements_names"]; + // Json::Value& FTSensorElementNames = FTSensor["elements_names"]; for (std::string ftElementName : ftElementNames) FTSensorElementNames.append(ftElementName); - Json::Value& FTSensorTimeStamp = FTSensor["timestamps"]; + // Json::Value& FTSensorTimeStamp = FTSensor["timestamps"]; FTSensorTimeStamp.append(time); } } @@ -1367,85 +1388,85 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() if(m_streamInertials) { // pack the data for the gyros - Json::Value& gyros = robotRealtime["gyros"]; + // Json::Value& gyros = robotRealtime["gyros"]; std::string gyroElementNames[] = {"omega_x", "omega_y", "omega_z"}; for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { - Json::Value& gyroSensor = gyros[sensorName]; - Json::Value& gyroSensorData = gyroSensor["data"]; + // Json::Value& gyroSensor = gyros[sensorName]; + // Json::Value& gyroSensorData = gyroSensor["data"]; if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { for (unsigned int i = 0; i < m_gyroBuffer.rows(); i++) gyroSensorData.append(m_gyroBuffer.coeff(i,0)); - Json::Value& gyroSensorElementNames = gyroSensor["elements_names"]; + // Json::Value& gyroSensorElementNames = gyroSensor["elements_names"]; for(std::string gyroElementName : gyroElementNames) gyroSensorElementNames.append(gyroElementName); - Json::Value& gyroSensorTimeStamp = gyroSensor["timestamps"]; + // Json::Value& gyroSensorTimeStamp = gyroSensor["timestamps"]; gyroSensorTimeStamp.append(time); } } // pack the data for the accelerometer - Json::Value& accelerometers = robotRealtime["accelerometers"]; + // Json::Value& accelerometers = robotRealtime["accelerometers"]; std::string accelerometerElementNames[] = {"a_x", "a_y", "a_z"}; for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { - Json::Value& accelerometerSensor = accelerometers[sensorName]; - Json::Value& accelerometerSensorData = accelerometerSensor["data"]; + // Json::Value& accelerometerSensor = accelerometers[sensorName]; + // Json::Value& accelerometerSensorData = accelerometerSensor["data"]; if(m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) { for (unsigned int i = 0; i < m_acceloremeterBuffer.rows(); i++) accelerometerSensorData.append(m_acceloremeterBuffer.coeff(i,0)); - Json::Value& accelerometerSensorElementNames = accelerometerSensor["elements_names"]; + // Json::Value& accelerometerSensorElementNames = accelerometerSensor["elements_names"]; for(std::string accelerometerElementName : accelerometerElementNames) accelerometerSensorElementNames.append(accelerometerElementName); - Json::Value& accelerometerSensorTimeStamp = accelerometerSensor["timestamps"]; + // Json::Value& accelerometerSensorTimeStamp = accelerometerSensor["timestamps"]; accelerometerSensorTimeStamp.append(time); } } // pack the data for the orientations - Json::Value& orientations = robotRealtime["orientations"]; + // Json::Value& orientations = robotRealtime["orientations"]; std::string orientationElementNames[] = {"r", "p", "y"}; for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { - Json::Value& orientationSensor = orientations[sensorName]; - Json::Value& orientationSensorData = orientationSensor["data"]; + // Json::Value& orientationSensor = orientations[sensorName]; + // Json::Value& orientationSensorData = orientationSensor["data"]; if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) { for (unsigned int i = 0; i < m_orientationBuffer.rows(); i++) orientationSensorData.append(m_orientationBuffer.coeff(i,0)); - Json::Value& orientationSensorElementNames = orientationSensor["elements_names"]; + // Json::Value& orientationSensorElementNames = orientationSensor["elements_names"]; for(std::string orientationElementName : orientationElementNames) orientationSensorElementNames.append(orientationElementName); - Json::Value& orientationSensorTimeStamp = orientationSensor["timestamps"]; + // Json::Value& orientationSensorTimeStamp = orientationSensor["timestamps"]; orientationSensorTimeStamp.append(time); } } // pack the data for the magnemetometer - Json::Value& mangetometers = robotRealtime["mangetometers"]; + // Json::Value& mangetometers = robotRealtime["mangetometers"]; std::string mangetometerElementNames[] = {"mag_x", "mag_y", "mag_z"}; for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { - Json::Value& mangetometerSensor = mangetometers[sensorName]; - Json::Value& mangetometerSensorData = mangetometerSensor["data"]; + // Json::Value& mangetometerSensor = mangetometers[sensorName]; + // Json::Value& mangetometerSensorData = mangetometerSensor["data"]; if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { for (unsigned int i = 0; i < m_magnemetometerBuffer.rows(); i++) mangetometerSensorData.append(m_magnemetometerBuffer.coeff(i,0)); - Json::Value& mangetomterSensorElementNames = mangetometerSensor["elements_names"]; + // Json::Value& mangetomterSensorElementNames = mangetometerSensor["elements_names"]; for(std::string mangetometerElementName : mangetometerElementNames) mangetomterSensorElementNames.append(mangetometerElementName); - Json::Value& mangetometerSensorTimeStamp = mangetometerSensor["timestamps"]; + // Json::Value& mangetometerSensorTimeStamp = mangetometerSensor["timestamps"]; mangetometerSensorTimeStamp.append(time); } } @@ -1453,19 +1474,19 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() // pack the data for the temperature if (m_streamTemperatureSensors) { - Json::Value& temperatures = robotRealtime["temperatures"]; + // Json::Value& temperatures = robotRealtime["temperatures"]; for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { - Json::Value& temperatureSensor = temperatures[sensorName]; - Json::Value& temperatureSensorData = temperatureSensor["data"]; + // Json::Value& temperatureSensor = temperatures[sensorName]; + // Json::Value& temperatureSensorData = temperatureSensor["data"]; if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) { temperatureSensorData.append(m_ftTemperatureBuffer); - Json::Value& temperatureSensorElementNames = temperatureSensor["elements_names"]; + // Json::Value& temperatureSensorElementNames = temperatureSensor["elements_names"]; temperatureSensorElementNames.append("temperature"); - Json::Value& temperatureSensorTimeStamp = temperatureSensor["timestamps"]; + // Json::Value& temperatureSensorTimeStamp = temperatureSensor["timestamps"]; temperatureSensorTimeStamp.append(time); } } @@ -1474,22 +1495,22 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() // pack the data for the cartesian wrenches if (m_streamCartesianWrenches) { - Json::Value& cartesianWrenches = robotRealtime["cartesian_wrenches"]; + // Json::Value& cartesianWrenches = robotRealtime["cartesian_wrenches"]; std::string cartesianElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { - Json::Value& cartesianWrench = cartesianWrenches[cartesianWrenchName]; - Json::Value& cartesianWrenchData = cartesianWrench["data"]; + // Json::Value& cartesianWrench = cartesianWrenches[cartesianWrenchName]; + // Json::Value& cartesianWrenchData = cartesianWrench["data"]; if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) { for (unsigned int i = 0; i < m_ftBuffer.rows(); i++) cartesianWrenchData.append(m_ftBuffer(i, 0)); - Json::Value& cartesianWrenchElementNames = cartesianWrench["elements_names"]; + // Json::Value& cartesianWrenchElementNames = cartesianWrench["elements_names"]; for(std::string cartesianElementName : cartesianElementNames) cartesianWrenchElementNames.append(cartesianElementName); - Json::Value& cartesianWrenchTimeStamp = cartesianWrench["timestamps"]; + // Json::Value& cartesianWrenchTimeStamp = cartesianWrench["timestamps"]; cartesianWrenchTimeStamp.append(time); } @@ -1497,7 +1518,7 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() } // pack the external data - Json::Value& flightData = robotRealtime["FlightData"]; + // Json::Value& flightData = robotRealtime["FlightData"]; for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); @@ -1511,11 +1532,14 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() } } } - Json::FastWriter fastWriter; - std::string jsonDataToSend = fastWriter.write(root); + // Json::FastWriter fastWriter; + // std::string jsonDataToSend = fastWriter.write(root); + + // realtimeOutput.addString(jsonDataToSend); + // realtimeLoggingPort.write(true); + */ - realtimeOutput.addString(jsonDataToSend); - realtimeLoggingPort.write(true); + vectorCollectionRTDataServer.sendData(); } @@ -1674,7 +1698,6 @@ void YarpRobotLoggerDevice::run() { std::lock_guard lock(signal.mutex); - externalSignalCollection = signal.port.read(false); if (externalSignalCollection != nullptr) { if (!signal.dataArrived) From 6ca1fe9fdd5baabea083e2ce9d29adc3b387c391 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 16 Jan 2024 15:17:40 +0100 Subject: [PATCH 17/83] Added get metadata method for pybind11 --- bindings/python/YarpUtilities/src/VectorsCollection.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index e24ce36118..dc9129344f 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -63,7 +63,13 @@ void CreateVectorsCollectionClient(pybind11::module& module) py::arg("handler")) .def("connect", &VectorsCollectionClient::connect) .def("disconnect", &VectorsCollectionClient::disconnect) - .def("getMetadata", &VectorsCollectionClient::getMetadata, py::arg("force_strict") = false) + .def("getMetadata", + [](VectorsCollectionClient& impl) -> std::map> + { + BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata metadata; + impl.getMetadata(metadata); + return metadata.vectors; + }) .def("readData", /*&VectorsCollectionClient::readData, py::arg("shouldWait") = true, py::return_value_policy::reference_internal); */ [](VectorsCollectionClient& impl, bool shouldWait) -> std::map> From 6284950911b4e41e25cde6fa689191a8c3385ddc Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 16 Jan 2024 15:18:20 +0100 Subject: [PATCH 18/83] Added metadata values --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 66cd581855..94e6632a9a 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1274,6 +1274,8 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() std::vector joints; m_robotSensorBridge->getJointsList(joints); vectorCollectionRTDataServer.populateMetadata("robot_realtime::description_list", joints); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::timestamps", {"timestamps"}); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::yarp_robot_name", {"robot_realtime"}); // confgure the metaData for the joints_state if(m_streamJointStates) { @@ -1342,6 +1344,8 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() { const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); + Eigen::Matrix timeData; + timeData << time; vectorCollectionRTDataServer.clearData(); From 5f4c3fb94ebaecae7d919f3e867453a0d1133232 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 16 Jan 2024 15:18:46 +0100 Subject: [PATCH 19/83] Got joint data sending over the vector Collection Network --- .../src/YarpRobotLoggerDevice.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 94e6632a9a..67ebf8e241 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1284,7 +1284,7 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::accelerations", joints); vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::torques", joints); } - +/* // configure the metadata for the FTs if(m_streamFTSensors) { @@ -1336,7 +1336,7 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() std::string fullCartesianWrenchName = "robot_realtime::cartesian_wrenches::" + cartesianWrenchName; vectorCollectionRTDataServer.populateMetadata(fullCartesianWrenchName, {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}); } - } + }*/ vectorCollectionRTDataServer.finalizeMetadata(); } @@ -1349,17 +1349,20 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() vectorCollectionRTDataServer.clearData(); + vectorCollectionRTDataServer.populateData("robot_realtime::timestamps", timeData); + if (m_streamJointStates) { if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::positions", m_jointSensorBuffer); - if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) + if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::velocities", m_jointSensorBuffer); if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::accelerations", m_jointSensorBuffer); if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::torques", m_jointSensorBuffer); } + std::cout << "Sending data to the visualizer" << std::endl; /* if (m_streamMotorStates) { @@ -1587,6 +1590,7 @@ std::vector YarpRobotLoggerDevice::tokenizeSubString(std::string in void YarpRobotLoggerDevice::run() { constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; + std::cout << "In the run loop" << std::endl; // get the data if (!m_robotSensorBridge->advance()) @@ -1697,7 +1701,8 @@ void YarpRobotLoggerDevice::run() } } - std::string signalFullName; + std::cout << "About to read from the external signals" << std::endl; + /*std::string signalFullName; for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); @@ -1736,6 +1741,7 @@ void YarpRobotLoggerDevice::run() } } } + std::cout << "Read from first round of external signals" << std::endl; for (auto& [name, signal] : m_vectorSignals) { @@ -1783,7 +1789,7 @@ void YarpRobotLoggerDevice::run() { break; } - } + }*/ SendDataToLoggerVisualizer(); } From edf8b458ba88d8d36bec09846b00cc549170e6e2 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 16 Jan 2024 17:37:07 +0100 Subject: [PATCH 20/83] Added more values to send over via the vector collection server --- .../src/YarpRobotLoggerDevice.cpp | 125 +++--------------- 1 file changed, 17 insertions(+), 108 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 67ebf8e241..0b39b8b0c6 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1284,7 +1284,7 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::accelerations", joints); vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::torques", joints); } -/* + // configure the metadata for the FTs if(m_streamFTSensors) { @@ -1324,7 +1324,7 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() { for (const auto& sensorName: m_robotSensorBridge->getTemperatureSensorsList()) { - std::string fullTemperatureSensorName = "robot_realtime::temperatures::" + sensorName; + std::string fullTemperatureSensorName = "robot_realtime::temperature::" + sensorName; vectorCollectionRTDataServer.populateMetadata(fullTemperatureSensorName, {"temperature"}); } } @@ -1333,11 +1333,11 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() { for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { - std::string fullCartesianWrenchName = "robot_realtime::cartesian_wrenches::" + cartesianWrenchName; + std::string fullCartesianWrenchName = "robot_realtime::cartesian_wrench::" + cartesianWrenchName; + std::cout << fullCartesianWrenchName << std::endl; vectorCollectionRTDataServer.populateMetadata(fullCartesianWrenchName, {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}); } - }*/ - + } vectorCollectionRTDataServer.finalizeMetadata(); } @@ -1363,7 +1363,7 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::torques", m_jointSensorBuffer); } std::cout << "Sending data to the visualizer" << std::endl; - /* + if (m_streamMotorStates) { @@ -1371,130 +1371,54 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() // pack the data for the FT Sensors if (m_streamFTSensors) { - // Json::Value& FTSensors = robotRealtime["FTs"]; - std::string ftElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) - { - // Json::Value& FTSensor = FTSensors[sensorName]; - // Json::Value& FTSensorData = FTSensor["data"]; - for (unsigned int i = 0; i < m_ftBuffer.rows(); i++) - FTSensorData.append(m_ftBuffer.coeff(i, 0)); - - // Json::Value& FTSensorElementNames = FTSensor["elements_names"]; - for (std::string ftElementName : ftElementNames) - FTSensorElementNames.append(ftElementName); - // Json::Value& FTSensorTimeStamp = FTSensor["timestamps"]; - FTSensorTimeStamp.append(time); - } + vectorCollectionRTDataServer.populateData("robot_realtime::FTs::" + sensorName, m_ftBuffer); } } // pack the data for the inertial measurements if(m_streamInertials) { - // pack the data for the gyros - // Json::Value& gyros = robotRealtime["gyros"]; - std::string gyroElementNames[] = {"omega_x", "omega_y", "omega_z"}; for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { - // Json::Value& gyroSensor = gyros[sensorName]; - // Json::Value& gyroSensorData = gyroSensor["data"]; if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) - { - for (unsigned int i = 0; i < m_gyroBuffer.rows(); i++) - gyroSensorData.append(m_gyroBuffer.coeff(i,0)); - - // Json::Value& gyroSensorElementNames = gyroSensor["elements_names"]; - for(std::string gyroElementName : gyroElementNames) - gyroSensorElementNames.append(gyroElementName); - - // Json::Value& gyroSensorTimeStamp = gyroSensor["timestamps"]; - gyroSensorTimeStamp.append(time); - } + vectorCollectionRTDataServer.populateData("robot_realtime::gyros::" + sensorName, m_gyroBuffer); } // pack the data for the accelerometer - // Json::Value& accelerometers = robotRealtime["accelerometers"]; - std::string accelerometerElementNames[] = {"a_x", "a_y", "a_z"}; for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { - // Json::Value& accelerometerSensor = accelerometers[sensorName]; - // Json::Value& accelerometerSensorData = accelerometerSensor["data"]; if(m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) - { - for (unsigned int i = 0; i < m_acceloremeterBuffer.rows(); i++) - accelerometerSensorData.append(m_acceloremeterBuffer.coeff(i,0)); - - // Json::Value& accelerometerSensorElementNames = accelerometerSensor["elements_names"]; - for(std::string accelerometerElementName : accelerometerElementNames) - accelerometerSensorElementNames.append(accelerometerElementName); - - // Json::Value& accelerometerSensorTimeStamp = accelerometerSensor["timestamps"]; - accelerometerSensorTimeStamp.append(time); - } + vectorCollectionRTDataServer.populateData("robot_realtime::accelerometers::" + sensorName, m_acceloremeterBuffer); } // pack the data for the orientations // Json::Value& orientations = robotRealtime["orientations"]; - std::string orientationElementNames[] = {"r", "p", "y"}; for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { - // Json::Value& orientationSensor = orientations[sensorName]; - // Json::Value& orientationSensorData = orientationSensor["data"]; if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) - { - for (unsigned int i = 0; i < m_orientationBuffer.rows(); i++) - orientationSensorData.append(m_orientationBuffer.coeff(i,0)); - - // Json::Value& orientationSensorElementNames = orientationSensor["elements_names"]; - for(std::string orientationElementName : orientationElementNames) - orientationSensorElementNames.append(orientationElementName); - - // Json::Value& orientationSensorTimeStamp = orientationSensor["timestamps"]; - orientationSensorTimeStamp.append(time); - } + vectorCollectionRTDataServer.populateData("robot_realtime::orientations::" + sensorName, m_orientationBuffer); } - // pack the data for the magnemetometer - // Json::Value& mangetometers = robotRealtime["mangetometers"]; - std::string mangetometerElementNames[] = {"mag_x", "mag_y", "mag_z"}; for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { - // Json::Value& mangetometerSensor = mangetometers[sensorName]; - // Json::Value& mangetometerSensorData = mangetometerSensor["data"]; if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) - { - for (unsigned int i = 0; i < m_magnemetometerBuffer.rows(); i++) - mangetometerSensorData.append(m_magnemetometerBuffer.coeff(i,0)); - - // Json::Value& mangetomterSensorElementNames = mangetometerSensor["elements_names"]; - for(std::string mangetometerElementName : mangetometerElementNames) - mangetomterSensorElementNames.append(mangetometerElementName); - - // Json::Value& mangetometerSensorTimeStamp = mangetometerSensor["timestamps"]; - mangetometerSensorTimeStamp.append(time); - } + vectorCollectionRTDataServer.populateData("robot_realtime::magnetometers::" + sensorName, m_magnemetometerBuffer); } } + // pack the data for the temperature if (m_streamTemperatureSensors) { - // Json::Value& temperatures = robotRealtime["temperatures"]; for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { - // Json::Value& temperatureSensor = temperatures[sensorName]; - // Json::Value& temperatureSensorData = temperatureSensor["data"]; if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) { - temperatureSensorData.append(m_ftTemperatureBuffer); - - // Json::Value& temperatureSensorElementNames = temperatureSensor["elements_names"]; - temperatureSensorElementNames.append("temperature"); - - // Json::Value& temperatureSensorTimeStamp = temperatureSensor["timestamps"]; - temperatureSensorTimeStamp.append(time); + Eigen::Matrix temperatureData; + temperatureData << m_ftTemperatureBuffer; + vectorCollectionRTDataServer.populateData("robot_realtime::temperature::" + sensorName, temperatureData); } } } @@ -1502,31 +1426,16 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() // pack the data for the cartesian wrenches if (m_streamCartesianWrenches) { - // Json::Value& cartesianWrenches = robotRealtime["cartesian_wrenches"]; - std::string cartesianElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { - // Json::Value& cartesianWrench = cartesianWrenches[cartesianWrenchName]; - // Json::Value& cartesianWrenchData = cartesianWrench["data"]; if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) - { - for (unsigned int i = 0; i < m_ftBuffer.rows(); i++) - cartesianWrenchData.append(m_ftBuffer(i, 0)); - - // Json::Value& cartesianWrenchElementNames = cartesianWrench["elements_names"]; - for(std::string cartesianElementName : cartesianElementNames) - cartesianWrenchElementNames.append(cartesianElementName); - - // Json::Value& cartesianWrenchTimeStamp = cartesianWrench["timestamps"]; - cartesianWrenchTimeStamp.append(time); - - } + vectorCollectionRTDataServer.populateData("robot_realtime::cartesian_wrench::" + cartesianWrenchName, m_ftBuffer); } } // pack the external data // Json::Value& flightData = robotRealtime["FlightData"]; - for (auto& [name, signal] : m_vectorsCollectionSignals) + /*for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); // collection is populated from the global variable From c25f25dd92efc53d83ac2ab03195388d39d93583 Mon Sep 17 00:00:00 2001 From: Nick Date: Wed, 17 Jan 2024 12:12:45 +0100 Subject: [PATCH 21/83] Cleaned up the code --- .../YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 0b39b8b0c6..e2efce5e9d 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -893,10 +893,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } } - std::cout << "Initializing vector collection server" << std::endl; ConfigureVectorCollectionServer(); - std::cout << "Vector collection server has been initalized" << std::endl; - ok = ok && m_bufferManager.setSaveCallback( [this](const std::string& filePrefix, @@ -1260,9 +1257,7 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() { auto loggerOption = std::make_shared(); - std::cout << "About to set the parameter for the vector collection server" << std::endl; loggerOption->setParameter("remote", "/testVectorCollections"); - std::cout << "About to configure the Collection Server" << std::endl; if (!vectorCollectionRTDataServer.initialize(loggerOption)) { std::cout << "Failed to initalize the vectorsCollectionServer" << std::endl; @@ -1334,7 +1329,6 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { std::string fullCartesianWrenchName = "robot_realtime::cartesian_wrench::" + cartesianWrenchName; - std::cout << fullCartesianWrenchName << std::endl; vectorCollectionRTDataServer.populateMetadata(fullCartesianWrenchName, {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}); } } @@ -1362,7 +1356,6 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::torques", m_jointSensorBuffer); } - std::cout << "Sending data to the visualizer" << std::endl; if (m_streamMotorStates) { @@ -1499,7 +1492,6 @@ std::vector YarpRobotLoggerDevice::tokenizeSubString(std::string in void YarpRobotLoggerDevice::run() { constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; - std::cout << "In the run loop" << std::endl; // get the data if (!m_robotSensorBridge->advance()) @@ -1610,7 +1602,6 @@ void YarpRobotLoggerDevice::run() } } - std::cout << "About to read from the external signals" << std::endl; /*std::string signalFullName; for (auto& [name, signal] : m_vectorsCollectionSignals) { From 5409fab9d29736146c8c556ae8420e04b18b2dee Mon Sep 17 00:00:00 2001 From: Nick Date: Wed, 17 Jan 2024 16:56:32 +0100 Subject: [PATCH 22/83] got plot scaling updating correctly --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index e2efce5e9d..6541e9e0ab 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1602,17 +1602,19 @@ void YarpRobotLoggerDevice::run() } } - /*std::string signalFullName; + std::string signalFullName; for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); + externalSignalCollection = signal.client.readData(false); if (externalSignalCollection != nullptr) { if (!signal.dataArrived) { for (const auto& [key, vector] : externalSignalCollection->vectors) { + std::cout << "External signal key: " << key << std::endl; signalFullName = signal.signalName + "::" + key; const auto& metadata = signal.metadata.vectors.find(key); if (metadata == signal.metadata.vectors.cend()) @@ -1689,7 +1691,7 @@ void YarpRobotLoggerDevice::run() { break; } - }*/ + } SendDataToLoggerVisualizer(); } From b83a7f6ed25f79aee71c1d5be54586c7db21839a Mon Sep 17 00:00:00 2001 From: Nick Date: Wed, 17 Jan 2024 19:18:23 +0100 Subject: [PATCH 23/83] Flight data streaming --- .../src/YarpRobotLoggerDevice.cpp | 34 ++++++++++++++++--- 1 file changed, 29 insertions(+), 5 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 6541e9e0ab..adbb6b5e0f 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1053,6 +1053,7 @@ void YarpRobotLoggerDevice::lookForExogenousSignals() wakeUpTime += lookForExogenousSignalPeriod; // try to connect to the exogenous signals + std::cout << "Now connecting to exogenous data" << std::endl; connectToExogeneous(m_vectorsCollectionSignals); connectToExogeneous(m_vectorSignals); @@ -1332,6 +1333,25 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() vectorCollectionRTDataServer.populateMetadata(fullCartesianWrenchName, {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}); } } + + for (auto& [name, signal] : m_vectorsCollectionSignals) + { + std::cout << "In the first loop" << std::endl; + + externalSignalCollection = signal.client.readData(false); + if (externalSignalCollection != nullptr) + { + for (const auto& [key, vector] : externalSignalCollection->vectors) + { + std::string signalFullName = "robot_realtime::" + signal.signalName + "::" + key; + std::cout << "External signal key: " << signalFullName << std::endl; + vectorCollectionRTDataServer.populateMetadata(signalFullName, {}); + } + } + } + + std::cout << "About to finalize metadata" << std::endl; + vectorCollectionRTDataServer.finalizeMetadata(); } @@ -1428,7 +1448,7 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() // pack the external data // Json::Value& flightData = robotRealtime["FlightData"]; - /*for (auto& [name, signal] : m_vectorsCollectionSignals) + for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); // collection is populated from the global variable @@ -1436,8 +1456,11 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() { for (const auto& [key, vector] : externalSignalCollection->vectors) { - std::vector keyTokens = tokenizeSubString(key, "::"); - PackFlightData(keyTokens, vector, flightData, time); + std::string fullSignalName = "robot_realtime::FlightData::" + key; + // std::cout << "populating: " << fullSignalName << std::endl; + vectorCollectionRTDataServer.populateData(fullSignalName, vector); + // std::vector keyTokens = tokenizeSubString(key, "::"); + // PackFlightData(keyTokens, vector, flightData, time); } } } @@ -1446,7 +1469,7 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() // realtimeOutput.addString(jsonDataToSend); // realtimeLoggingPort.write(true); - */ + vectorCollectionRTDataServer.sendData(); @@ -1614,7 +1637,6 @@ void YarpRobotLoggerDevice::run() { for (const auto& [key, vector] : externalSignalCollection->vectors) { - std::cout << "External signal key: " << key << std::endl; signalFullName = signal.signalName + "::" + key; const auto& metadata = signal.metadata.vectors.find(key); if (metadata == signal.metadata.vectors.cend()) @@ -1626,6 +1648,8 @@ void YarpRobotLoggerDevice::run() m_bufferManager.addChannel({signalFullName, {vector.size(), 1}}); } else { + std::cout << "MetaData for: " << key << std::endl; + std::cout << metadata->second[0] << std::endl; // if the metadata is found we use it m_bufferManager.addChannel({signalFullName, // {vector.size(), 1}, From 5eabf65fda701d55f7d9b2fb3ed180e1ff42654a Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 18 Jan 2024 12:17:25 +0100 Subject: [PATCH 24/83] Got metadata naming correct --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 4 - .../src/YarpRobotLoggerDevice.cpp | 85 +++++-------------- 2 files changed, 22 insertions(+), 67 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 854cef2d8b..7a0a5aa9b4 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -208,10 +208,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void SendDataToLoggerVisualizer(); void ConfigureVectorCollectionServer(); - - void PackFlightData(std::vector keyTokens, const std::vector& values, Json::Value& jsonValueToPack, const double& time); - - std::vector tokenizeSubString(std::string input, std::string delimiter); }; } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index adbb6b5e0f..f906198324 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1053,7 +1053,6 @@ void YarpRobotLoggerDevice::lookForExogenousSignals() wakeUpTime += lookForExogenousSignalPeriod; // try to connect to the exogenous signals - std::cout << "Now connecting to exogenous data" << std::endl; connectToExogeneous(m_vectorsCollectionSignals); connectToExogeneous(m_vectorSignals); @@ -1281,6 +1280,14 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::torques", joints); } + // if(m_streamMotorStates) + // { + // vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::positions", joints); + // vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::velocities", joints); + // vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::accelerations", joints); + // vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::currents", joints); + // } + // configure the metadata for the FTs if(m_streamFTSensors) { @@ -1336,22 +1343,17 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() for (auto& [name, signal] : m_vectorsCollectionSignals) { - std::cout << "In the first loop" << std::endl; - externalSignalCollection = signal.client.readData(false); if (externalSignalCollection != nullptr) { for (const auto& [key, vector] : externalSignalCollection->vectors) { std::string signalFullName = "robot_realtime::" + signal.signalName + "::" + key; - std::cout << "External signal key: " << signalFullName << std::endl; vectorCollectionRTDataServer.populateMetadata(signalFullName, {}); } } } - std::cout << "About to finalize metadata" << std::endl; - vectorCollectionRTDataServer.finalizeMetadata(); } @@ -1377,10 +1379,18 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::torques", m_jointSensorBuffer); } - if (m_streamMotorStates) - { - - } + // if (m_streamMotorStates) + // { + // if(m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) + // vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::positions", m_jointSensorBuffer); + // if(m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) + // vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::velocities", m_jointSensorBuffer); + // if(m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) + // vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::accelerations", m_jointSensorBuffer); + // if(m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) + // vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::currents", m_jointSensorBuffer); + + // } // pack the data for the FT Sensors if (m_streamFTSensors) { @@ -1408,7 +1418,6 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() } // pack the data for the orientations - // Json::Value& orientations = robotRealtime["orientations"]; for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) @@ -1447,7 +1456,6 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() } // pack the external data - // Json::Value& flightData = robotRealtime["FlightData"]; for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); @@ -1456,62 +1464,16 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() { for (const auto& [key, vector] : externalSignalCollection->vectors) { - std::string fullSignalName = "robot_realtime::FlightData::" + key; - // std::cout << "populating: " << fullSignalName << std::endl; + std::string fullSignalName = "robot_realtime::" + signal.signalName + "::" + key; vectorCollectionRTDataServer.populateData(fullSignalName, vector); - // std::vector keyTokens = tokenizeSubString(key, "::"); - // PackFlightData(keyTokens, vector, flightData, time); } } } - // Json::FastWriter fastWriter; - // std::string jsonDataToSend = fastWriter.write(root); - - // realtimeOutput.addString(jsonDataToSend); - // realtimeLoggingPort.write(true); - vectorCollectionRTDataServer.sendData(); } -void YarpRobotLoggerDevice::PackFlightData(std::vector keyTokens, const std::vector& values, Json::Value& jsonValueToPack, const double& time) -{ - if(keyTokens.size() == 1) - { - Json::Value& jsonFinalValueToPack = jsonValueToPack[keyTokens[0]]; - Json::Value& jsonFinalValueData = jsonFinalValueToPack["data"]; - for(unsigned int i = 0; i < values.size(); i++) - jsonFinalValueData.append(values[i]); - - Json::Value& jsonFinalValueTimeStamp = jsonFinalValueToPack["timestamps"]; - jsonFinalValueTimeStamp.append(time); - - return; - } - - - Json::Value& subValuesToPack = jsonValueToPack[keyTokens[0]]; - std::vector subTokens = {keyTokens.begin() + 1, keyTokens.end()}; - PackFlightData(subTokens, values, subValuesToPack, time); -} - -std::vector YarpRobotLoggerDevice::tokenizeSubString(std::string input, std::string delimiter) -{ - size_t position = 0; - std::string token; - std::vector tokens; - while ((position = input.find(delimiter)) != std::string::npos) - { - token = input.substr(0, position); - tokens.push_back(token); - input.erase(0, position + delimiter.length()); - } - tokens.push_back(input); - - return tokens; -} - void YarpRobotLoggerDevice::run() { constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; @@ -1630,7 +1592,7 @@ void YarpRobotLoggerDevice::run() { std::lock_guard lock(signal.mutex); - externalSignalCollection = signal.client.readData(false); + externalSignalCollection = signal.client.readData(false); // was false if (externalSignalCollection != nullptr) { if (!signal.dataArrived) @@ -1648,8 +1610,6 @@ void YarpRobotLoggerDevice::run() m_bufferManager.addChannel({signalFullName, {vector.size(), 1}}); } else { - std::cout << "MetaData for: " << key << std::endl; - std::cout << metadata->second[0] << std::endl; // if the metadata is found we use it m_bufferManager.addChannel({signalFullName, // {vector.size(), 1}, @@ -1667,7 +1627,6 @@ void YarpRobotLoggerDevice::run() } } } - std::cout << "Read from first round of external signals" << std::endl; for (auto& [name, signal] : m_vectorSignals) { From 554fa1e3e5945b24e2bddd69bd95196a3ed04adc Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 18 Jan 2024 12:33:54 +0100 Subject: [PATCH 25/83] Added ability to stream motor states --- .../src/YarpRobotLoggerDevice.cpp | 38 +++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index f906198324..fff38ce0b8 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1280,13 +1280,13 @@ void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::torques", joints); } - // if(m_streamMotorStates) - // { - // vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::positions", joints); - // vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::velocities", joints); - // vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::accelerations", joints); - // vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::currents", joints); - // } + if(m_streamMotorStates) + { + vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::positions", joints); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::velocities", joints); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::accelerations", joints); + vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::currents", joints); + } // configure the metadata for the FTs if(m_streamFTSensors) @@ -1379,18 +1379,18 @@ void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::torques", m_jointSensorBuffer); } - // if (m_streamMotorStates) - // { - // if(m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) - // vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::positions", m_jointSensorBuffer); - // if(m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) - // vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::velocities", m_jointSensorBuffer); - // if(m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) - // vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::accelerations", m_jointSensorBuffer); - // if(m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) - // vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::currents", m_jointSensorBuffer); - - // } + if (m_streamMotorStates) + { + if(m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) + vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::positions", m_jointSensorBuffer); + if(m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) + vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::velocities", m_jointSensorBuffer); + if(m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) + vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::accelerations", m_jointSensorBuffer); + if(m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) + vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::currents", m_jointSensorBuffer); + + } // pack the data for the FT Sensors if (m_streamFTSensors) { From f45bfb5bbcbacd79927cdcd5c4610405fef9e4fa Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 18 Jan 2024 15:40:38 +0100 Subject: [PATCH 26/83] Cleaned up the code a bit --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index fff38ce0b8..385e2fc5f1 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -107,9 +107,6 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); - // for realtime logging - if (streamRealTime) - realtimeLoggingPort.open("/YARPRobotLoggerRT:o"); } YarpRobotLoggerDevice::YarpRobotLoggerDevice() @@ -123,8 +120,6 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); - if (streamRealTime) - realtimeLoggingPort.open("/YARPRobotLoggerRT:o"); } YarpRobotLoggerDevice::~YarpRobotLoggerDevice() = default; From ebf9d7697a927df981a0819086bfe5698cf917bc Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 18 Jan 2024 15:50:01 +0100 Subject: [PATCH 27/83] Removed whitespace in Module.cpp --- bindings/python/YarpUtilities/src/Module.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/bindings/python/YarpUtilities/src/Module.cpp b/bindings/python/YarpUtilities/src/Module.cpp index 7f24495cce..2187217a9d 100644 --- a/bindings/python/YarpUtilities/src/Module.cpp +++ b/bindings/python/YarpUtilities/src/Module.cpp @@ -21,7 +21,6 @@ void CreateModule(pybind11::module& module) CreateVectorsCollectionServer(module); CreateVectorsCollectionClient(module); - } } // namespace IK } // namespace bindings From 9def9a5405d979a3bfedd7cf9a33ffbc8e753ec6 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 18 Jan 2024 15:50:28 +0100 Subject: [PATCH 28/83] Removed whitespace from VectorsCollection.cpp --- bindings/python/YarpUtilities/src/VectorsCollection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index dc9129344f..11eb8f79bc 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -63,7 +63,7 @@ void CreateVectorsCollectionClient(pybind11::module& module) py::arg("handler")) .def("connect", &VectorsCollectionClient::connect) .def("disconnect", &VectorsCollectionClient::disconnect) - .def("getMetadata", + .def("getMetadata", [](VectorsCollectionClient& impl) -> std::map> { BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata metadata; From d92f7f4392f6d2c5b42a7494948cb755fe16bf80 Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 18 Jan 2024 17:22:45 +0100 Subject: [PATCH 29/83] Cleaned up the code, removed debug statements/comments --- bindings/python/YarpUtilities/src/VectorsCollection.cpp | 3 +-- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 4 +--- .../YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 9 +-------- 3 files changed, 3 insertions(+), 13 deletions(-) diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index dc9129344f..ac19281781 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -70,8 +70,7 @@ void CreateVectorsCollectionClient(pybind11::module& module) impl.getMetadata(metadata); return metadata.vectors; }) - .def("readData", /*&VectorsCollectionClient::readData, py::arg("shouldWait") = true, - py::return_value_policy::reference_internal); */ + .def("readData", [](VectorsCollectionClient& impl, bool shouldWait) -> std::map> { BipedalLocomotion::YarpUtilities::VectorsCollection* collection = impl.readData(shouldWait); diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 7a0a5aa9b4..4c8d70aed7 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -13,9 +13,7 @@ #include #include #include -#include -#include -#include + #include #include diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 385e2fc5f1..86fc90981b 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include @@ -34,9 +33,6 @@ #include #include #include -#include - -#include #include #include @@ -988,8 +984,6 @@ void YarpRobotLoggerDevice::unpackIMU(Eigen::Ref signal, void YarpRobotLoggerDevice::lookForExogenousSignals() { - std::cout << "Looking for exogenous signals" << std::endl; - yarp::profiler::NetworkProfiler::ports_name_set yarpPorts; using namespace std::chrono_literals; @@ -1074,7 +1068,6 @@ bool YarpRobotLoggerDevice::hasSubstring(const std::string& str, void YarpRobotLoggerDevice::lookForNewLogs() { - std::cout << "Looking for new logs" << std::endl; using namespace std::chrono_literals; yarp::profiler::NetworkProfiler::ports_name_set yarpPorts; constexpr auto textLoggingPortPrefix = "/log/"; @@ -1587,7 +1580,7 @@ void YarpRobotLoggerDevice::run() { std::lock_guard lock(signal.mutex); - externalSignalCollection = signal.client.readData(false); // was false + externalSignalCollection = signal.client.readData(false); if (externalSignalCollection != nullptr) { if (!signal.dataArrived) From c3c79adf8f8b028d5aa7a0e6125282466447e4b0 Mon Sep 17 00:00:00 2001 From: Nick Date: Fri, 19 Jan 2024 14:54:50 +0100 Subject: [PATCH 30/83] Reorganized the code and removed hardcoded values --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 4 - .../YarpRobotLoggerDeviceVariableTreeNames.h | 56 ++ .../src/YarpRobotLoggerDevice.cpp | 660 +++++++++--------- 3 files changed, 386 insertions(+), 334 deletions(-) create mode 100644 devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 4c8d70aed7..fb9cebfb98 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -171,10 +171,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::vector m_textLoggingSubnames; std::vector m_codeStatusCmdPrefixes; - // for realtime data streaming - bool streamRealTime{true}; - yarp::os::BufferedPort realtimeLoggingPort; - BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection; std::mutex m_bufferManagerMutex; robometry::BufferManager m_bufferManager; diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h new file mode 100644 index 0000000000..9028e4c126 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h @@ -0,0 +1,56 @@ +#ifndef BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H +#include + +#define BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H + +#define TREE_DELIM "::" + +#define ROBOT_RT_ROOT_NAME "robot_realtime" + + +#define JOINT_STATE_POSITIONS_NAME "joints_state::positions" +#define JOINT_STATE_VELOCITIES_NAME "joints_state::velocities" +#define JOINT_STATE_ACCLERATIONS_NAME "joints_state::accelerations" +#define JOINT_STATE_TORQUES_NAME "joints_state::torques" + +#define MOTOR_STATE_POSITIONS_NAME "motors_state::positions" +#define MOTOR_STATE_VELOCITIES_NAME "motors_state::velocities" +#define MOTOR_STATE_ACCELERATIONS_NAME "motors_state::acclerations" +#define MOTOR_STATE_CURRENTS_NAME "motors_state::currents" +#define MOTOR_STATE_PWM_NAME "motors_state::PWM" + +#define MOTOR_STATE_PIDS_NAME "PIDs" + +#define FTS_NAME "FTs" + +const std::string FTElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; + +#define GYROS_NAME "gyros" +const std::string GyroElementNames[] = {"omega_x", "omega_y", "omega_z"}; + +#define ACCELEROMETERS_NAME "accelerometers" +const std::string AccelerometerElementNames[] = {"a_x", "a_y", "a_z"}; + +#define ORIENTATIONS_NAME "orientations" +const std::string OrientationElementNames[] = {"r", "p", "y"}; + +#define MAGNETOMETERS_NAME "magnetometers" +const std::string MagnetometerElementNames[] = {"mag_x", "mag_y", "mag_z"}; + +#define CARTESIAN_WRENCHES_NAME "cartesian_wrenches" +const std::string CartesianWrenchNames[] = {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}; + +#define TEMPERATURE_NAME "temperatures" +const std::string TemperatureNames[] = {"temperature"}; + +#define YARP_NAME "yarp_robot_name" + +#define ROBOT_DESCRIPTON_LIST "description_list" + +#define TIMESTAMPS_NAME "timestamps" + +#endif + + + + diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 86fc90981b..d3a81b8536 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -611,6 +612,15 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { constexpr auto logPrefix = "[YarpRobotLoggerDevice::attachAll]"; + bool ok = true; + // open the TextLogging port + ok = ok && m_textLoggingPort.open(m_textLoggingPortName); + // run the thread + m_lookForNewLogsThread = std::thread([this] { this->lookForNewLogs(); }); + + // run the thread for reading the exogenous signals + m_lookForNewExogenousSignalThread = std::thread([this] { this->lookForExogenousSignals(); }); + if (!m_robotSensorBridge->setDriversList(poly)) { log()->error("{} Could not attach drivers list to sensor bridge.", logPrefix); @@ -639,45 +649,101 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) return false; } + // initialize the RT Logger Vector Collection Server + auto loggerOption = std::make_shared(); + loggerOption->setParameter("remote", "/testVectorCollections"); + if (!vectorCollectionRTDataServer.initialize(loggerOption)) + { + log()->error("Failed to initalize the vectorsCollectionServer", logPrefix); + return false; + } + char* tmp = std::getenv("YARP_ROBOT_NAME"); + std::string metadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(YARP_NAME); + vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); + + metadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); + vectorCollectionRTDataServer.populateMetadata(metadataName, {TIMESTAMPS_NAME}); + + const unsigned dofs = joints.size(); m_bufferManager.setDescriptionList(joints); + std::string rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ROBOT_DESCRIPTON_LIST); + vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - bool ok = true; // prepare the telemetry if (m_streamJointStates) { - ok = ok && m_bufferManager.addChannel({"joints_state::positions", {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({"joints_state::velocities", {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({"joints_state::accelerations", {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({"joints_state::torques", {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({JOINT_STATE_POSITIONS_NAME, {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({JOINT_STATE_VELOCITIES_NAME, {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({JOINT_STATE_ACCLERATIONS_NAME, {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({JOINT_STATE_TORQUES_NAME, {dofs, 1}, joints}); + + // populate metadata for joint positions + rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_POSITIONS_NAME); + vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + + // populate metadata for joint velocities + rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_VELOCITIES_NAME); + vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + + // populate metadata for joint accelerations + rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_ACCLERATIONS_NAME); + vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + + // populate metadata for joint torques + rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_TORQUES_NAME); + vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); } if (m_streamMotorStates) { - ok = ok && m_bufferManager.addChannel({"motors_state::positions", {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({"motors_state::velocities", {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({"motors_state::accelerations", {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({"motors_state::currents", {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({MOTOR_STATE_POSITIONS_NAME, {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({MOTOR_STATE_VELOCITIES_NAME, {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({MOTOR_STATE_ACCELERATIONS_NAME, {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({MOTOR_STATE_CURRENTS_NAME, {dofs, 1}, joints}); + + // populate metadata for motor state positions + rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_POSITIONS_NAME); + vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + + // populate metadata for motor state velocities + rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_VELOCITIES_NAME); + vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + + // populate metadata for motor state accelerations + rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_ACCELERATIONS_NAME); + vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + + // populate metadata for motor state currents + rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); + vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); } if (m_streamMotorPWM) { - ok = ok && m_bufferManager.addChannel({"motors_state::PWM", {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({MOTOR_STATE_PWM_NAME, {dofs, 1}, joints}); } if (m_streamPIDs) { - ok = ok && m_bufferManager.addChannel({"PIDs", {dofs, 1}, joints}); + ok = ok && m_bufferManager.addChannel({MOTOR_STATE_PIDS_NAME, {dofs, 1}, joints}); } if (m_streamFTSensors) { for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { + std::string fullFTSensorName = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; ok = ok - && m_bufferManager.addChannel({"FTs::" + sensorName, + && m_bufferManager.addChannel({fullFTSensorName, {6, 1}, // - {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}}); + {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}}); + + std::string rtFullFTSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + + std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; + + vectorCollectionRTDataServer.populateMetadata(rtFullFTSensorName, {FTElementNames[0], FTElementNames[1], FTElementNames[2], + FTElementNames[3], FTElementNames[4], FTElementNames[5]}); } } @@ -685,60 +751,94 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { + std::string fullGyroSensorName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; ok = ok - && m_bufferManager.addChannel({"gyros::" + sensorName, + && m_bufferManager.addChannel({fullGyroSensorName, {3, 1}, // - {"omega_x", "omega_y", "omega_z"}}); + {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}}); + + std::string rtFullGyroSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateMetadata(rtFullGyroSensorName, {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { + std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; ok = ok - && m_bufferManager.addChannel({"accelerometers::" + sensorName, + && m_bufferManager.addChannel({fullAccelerometerSensorName, {3, 1}, // - {"a_x", "a_y", "a_z"}}); + {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}}); + + std::string rtFullAccelerometerSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateMetadata(rtFullAccelerometerSensorName, {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { + std::string fullOrientationsSensorName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + ok = ok - && m_bufferManager.addChannel({"orientations::" + sensorName, + && m_bufferManager.addChannel({fullOrientationsSensorName, {3, 1}, // - {"r", "p", "y"}}); + {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}}); + + std::string rtFullOrientationSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateMetadata(rtFullOrientationSensorName, {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { + std::string fullMagnetometersSensorName = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; ok = ok - && m_bufferManager.addChannel({"magnetometers::" + sensorName, + && m_bufferManager.addChannel({fullMagnetometersSensorName, {3, 1}, // - {"mag_x", "mag_y", "mag_z"}}); + {MagnetometerElementNames[0], MagnetometerElementNames[1], MagnetometerElementNames[2]}}); + + std::string rtFullMagnetometersSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateMetadata(rtFullMagnetometersSensorName, {MagnetometerElementNames[0], MagnetometerElementNames[1], MagnetometerElementNames[2]}); } // an IMU contains a gyro accelerometer and an orientation sensor for (const auto& sensorName : m_robotSensorBridge->getIMUsList()) { + std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullGyroSensorName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullOrientationsSensorName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; ok = ok - && m_bufferManager.addChannel({"accelerometers::" + sensorName, + && m_bufferManager.addChannel({fullAccelerometerSensorName, {3, 1}, // - {"a_x", "a_y", "a_z"}}) - && m_bufferManager.addChannel({"gyros::" + sensorName, + {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}}) + && m_bufferManager.addChannel({fullGyroSensorName, {3, 1}, // - {"omega_x", "omega_y", "omega_z"}}) - && m_bufferManager.addChannel({"orientations::" + sensorName, + {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}}) + && m_bufferManager.addChannel({fullOrientationsSensorName, {3, 1}, // - {"r", "p", "y"}}); + {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}}); + + std::string rtFullAccelerometerSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateMetadata(rtFullAccelerometerSensorName, {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}); + + std::string rtFullGyroSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateMetadata(rtFullGyroSensorName, {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}); + + std::string rtFullOrientationSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateMetadata(rtFullOrientationSensorName, {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}); } } if (m_streamCartesianWrenches) { - for (const auto& sensorName : m_robotSensorBridge->getCartesianWrenchesList()) + for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { + std::string fullCartesianWrenchName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; ok = ok - && m_bufferManager.addChannel({"cartesian_wrenches::" + sensorName, + && m_bufferManager.addChannel({fullCartesianWrenchName, {6, 1}, // - {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}}); + {CartesianWrenchNames[0], CartesianWrenchNames[1], CartesianWrenchNames[2], CartesianWrenchNames[3], CartesianWrenchNames[4], CartesianWrenchNames[5]}}); + + std::string rtFullCartesianWrenchName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; + vectorCollectionRTDataServer.populateMetadata(rtFullCartesianWrenchName, {CartesianWrenchNames[0], CartesianWrenchNames[1], CartesianWrenchNames[2], + CartesianWrenchNames[3], CartesianWrenchNames[4], CartesianWrenchNames[5]}); } } @@ -746,23 +846,72 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { + std::string fullTemperatureSensorName = std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; ok = ok - && m_bufferManager.addChannel({"temperatures::" + sensorName, + && m_bufferManager.addChannel({fullTemperatureSensorName, {1, 1}, // - {"temperature"}}); + {TemperatureNames[0]}}); + std::string rtFullTemperatureSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateMetadata(rtFullTemperatureSensorName, {TemperatureNames[0]}); + } + } + std::string signalName = ""; + std::string rtSignalName = ""; + for (auto& [name, signal] : m_vectorsCollectionSignals) + { + std::lock_guard lock(signal.mutex); + BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection = signal.client.readData(false); + if (externalSignalCollection != nullptr) + { + for (const auto& [key, vector] : externalSignalCollection->vectors) + { + signalName = signal.signalName + std::string(TREE_DELIM) + 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, + signalName); + m_bufferManager.addChannel({signalName, {vector.size(), 1}}); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; + + vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); + } else + { + // if the metadata is found we use it + m_bufferManager.addChannel({signalName, // + {vector.size(), 1}, + metadata->second}); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; + vectorCollectionRTDataServer.populateMetadata(rtSignalName, {metadata->second}); + } + } + } + } + + for (auto& [name, signal] : m_vectorSignals) + { + std::lock_guard lock(signal.mutex); + yarp::sig::Vector* vector = signal.port.read(false); + if (vector != nullptr) + { + if (!signal.dataArrived) + { + m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); + + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signal.signalName; + vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); + signal.dataArrived = true; + } } } + vectorCollectionRTDataServer.finalizeMetadata(); + // resize the temporary vectors m_jointSensorBuffer.resize(dofs); - // open the TextLogging port - ok = ok && m_textLoggingPort.open(m_textLoggingPortName); - // run the thread - m_lookForNewLogsThread = std::thread([this] { this->lookForNewLogs(); }); - - // run the thread for reading the exogenous signals - m_lookForNewExogenousSignalThread = std::thread([this] { this->lookForExogenousSignals(); }); // The user can avoid to record the camera if (m_cameraBridge != nullptr) @@ -884,7 +1033,6 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } } - ConfigureVectorCollectionServer(); ok = ok && m_bufferManager.setSaveCallback( [this](const std::string& filePrefix, @@ -1242,312 +1390,187 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit } } -void YarpRobotLoggerDevice::ConfigureVectorCollectionServer() + +void YarpRobotLoggerDevice::run() { - auto loggerOption = std::make_shared(); - loggerOption->setParameter("remote", "/testVectorCollections"); - if (!vectorCollectionRTDataServer.initialize(loggerOption)) - { - std::cout << "Failed to initalize the vectorsCollectionServer" << std::endl; - return; - } - char* tmp = std::getenv("YARP_ROBOT_NAME"); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::yarp_robot_name", {std::string(tmp)}); + constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; + vectorCollectionRTDataServer.clearData(); - std::vector joints; - m_robotSensorBridge->getJointsList(joints); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::description_list", joints); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::timestamps", {"timestamps"}); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::yarp_robot_name", {"robot_realtime"}); - // confgure the metaData for the joints_state - if(m_streamJointStates) + // get the data + if (!m_robotSensorBridge->advance()) { - vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::positions", joints); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::velocities", joints); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::accelerations", joints); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::joints_state::torques", joints); + log()->error("{} Could not advance sensor bridge.", logPrefix); } - if(m_streamMotorStates) - { - vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::positions", joints); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::velocities", joints); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::accelerations", joints); - vectorCollectionRTDataServer.populateMetadata("robot_realtime::motors_state::currents", joints); - } + const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); + + std::lock_guard lock(m_bufferManagerMutex); - // configure the metadata for the FTs - if(m_streamFTSensors) + Eigen::Matrix timeData; + timeData << time; + + std::string signalName = ""; + std::string rtSignalName = ""; + + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, timeData); + + if (m_streamJointStates) { - for (const auto& sensorName: m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) + if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) + { + m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_POSITIONS_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_POSITIONS_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + } + if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) + { + m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_VELOCITIES_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_VELOCITIES_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + } + if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) + { + m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_ACCLERATIONS_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_ACCLERATIONS_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + } + if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { - std::string fullFTSensorName = "robot_realtime::FTs::" + sensorName; - vectorCollectionRTDataServer.populateMetadata(fullFTSensorName, {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}); + m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_TORQUES_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_TORQUES_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } } - // configure the metadata for the inertail measurments - if (m_streamInertials) + if (m_streamMotorStates) { - for (const auto& sensorName: m_robotSensorBridge->getGyroscopesList()) + if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { - std::string fullGyroSensorName = "robot_realtime::gyros::" + sensorName; - vectorCollectionRTDataServer.populateMetadata(fullGyroSensorName, {"omega_x", "omega_y", "omega_z"}); + m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_POSITIONS_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_POSITIONS_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } - for (const auto& sensorName: m_robotSensorBridge->getLinearAccelerometersList()) + if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { - std::string fullAccelerometerSensorName = "robot_realtime::accelerometers::" + sensorName; - vectorCollectionRTDataServer.populateMetadata(fullAccelerometerSensorName, {"a_x", "a_y", "a_z"}); + m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_VELOCITIES_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_VELOCITIES_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } - for (const auto& sensorName: m_robotSensorBridge->getOrientationSensorsList()) + if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { - std::string fullOrientationSensorName = "robot_realtime::orientations::" + sensorName; - vectorCollectionRTDataServer.populateMetadata(fullOrientationSensorName, {"r", "p", "y"}); + m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_ACCELERATIONS_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_ACCELERATIONS_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } - for (const auto& sensorName: m_robotSensorBridge->getMagnetometersList()) + if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { - std::string fullMagnetometersSensorName = "robot_realtime::magnetometers::" + sensorName; - vectorCollectionRTDataServer.populateMetadata(fullMagnetometersSensorName, {"mag_x", "mag_y", "mag_z"}); + m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_CURRENTS_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } } - if(m_streamTemperatureSensors) + if (m_streamMotorPWM) { - for (const auto& sensorName: m_robotSensorBridge->getTemperatureSensorsList()) + if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) { - std::string fullTemperatureSensorName = "robot_realtime::temperature::" + sensorName; - vectorCollectionRTDataServer.populateMetadata(fullTemperatureSensorName, {"temperature"}); + m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_PWM_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } } - if (m_streamCartesianWrenches) + if (m_streamPIDs) { - for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) + if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) { - std::string fullCartesianWrenchName = "robot_realtime::cartesian_wrench::" + cartesianWrenchName; - vectorCollectionRTDataServer.populateMetadata(fullCartesianWrenchName, {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}); + m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_PIDS_NAME); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_PIDS_NAME); + vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } } - for (auto& [name, signal] : m_vectorsCollectionSignals) + if (m_streamFTSensors) { - externalSignalCollection = signal.client.readData(false); - if (externalSignalCollection != nullptr) + for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { - for (const auto& [key, vector] : externalSignalCollection->vectors) + if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { - std::string signalFullName = "robot_realtime::" + signal.signalName + "::" + key; - vectorCollectionRTDataServer.populateMetadata(signalFullName, {}); + signalName = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; + m_bufferManager.push_back(m_ftBuffer, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateData(rtSignalName, m_ftBuffer); } } } - vectorCollectionRTDataServer.finalizeMetadata(); -} - -void YarpRobotLoggerDevice::SendDataToLoggerVisualizer() -{ - const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); - Eigen::Matrix timeData; - timeData << time; - - vectorCollectionRTDataServer.clearData(); - - vectorCollectionRTDataServer.populateData("robot_realtime::timestamps", timeData); - - if (m_streamJointStates) - { - if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::positions", m_jointSensorBuffer); - if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::velocities", m_jointSensorBuffer); - if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::accelerations", m_jointSensorBuffer); - if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::joints_state::torques", m_jointSensorBuffer); - } - - if (m_streamMotorStates) - { - if(m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::positions", m_jointSensorBuffer); - if(m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::velocities", m_jointSensorBuffer); - if(m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::accelerations", m_jointSensorBuffer); - if(m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::motors_state::currents", m_jointSensorBuffer); - - } - // pack the data for the FT Sensors - if (m_streamFTSensors) + if (m_streamTemperatureSensors) { - for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) + for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { - if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::FTs::" + sensorName, m_ftBuffer); + if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) + { + signalName = std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; + m_bufferManager.push_back({m_ftTemperatureBuffer}, time, signalName); + + Eigen::Matrix temperatureData; + temperatureData << m_ftTemperatureBuffer; + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateData(rtSignalName, temperatureData); + } } } - // pack the data for the inertial measurements if(m_streamInertials) { for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::gyros::" + sensorName, m_gyroBuffer); + { + signalName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + m_bufferManager.push_back(m_gyroBuffer, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateData(rtSignalName, m_gyroBuffer); + } } // pack the data for the accelerometer for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { if(m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::accelerometers::" + sensorName, m_acceloremeterBuffer); + { + signalName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + m_bufferManager.push_back(m_acceloremeterBuffer, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateData(rtSignalName, m_acceloremeterBuffer); + } } // pack the data for the orientations for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::orientations::" + sensorName, m_orientationBuffer); - } - - for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) - { - if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::magnetometers::" + sensorName, m_magnemetometerBuffer); - } - } - - // pack the data for the temperature - if (m_streamTemperatureSensors) - { - for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) - { - if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) { - Eigen::Matrix temperatureData; - temperatureData << m_ftTemperatureBuffer; - vectorCollectionRTDataServer.populateData("robot_realtime::temperature::" + sensorName, temperatureData); + signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + m_bufferManager.push_back(m_orientationBuffer, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateData(rtSignalName, m_orientationBuffer); } } - } - // pack the data for the cartesian wrenches - if (m_streamCartesianWrenches) - { - for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) - { - if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) - vectorCollectionRTDataServer.populateData("robot_realtime::cartesian_wrench::" + cartesianWrenchName, m_ftBuffer); - } - } - - // pack the external data - for (auto& [name, signal] : m_vectorsCollectionSignals) - { - std::lock_guard lock(signal.mutex); - // collection is populated from the global variable - if (externalSignalCollection != nullptr) + for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { - for (const auto& [key, vector] : externalSignalCollection->vectors) + if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { - std::string fullSignalName = "robot_realtime::" + signal.signalName + "::" + key; - vectorCollectionRTDataServer.populateData(fullSignalName, vector); + signalName = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + m_bufferManager.push_back(m_magnemetometerBuffer, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateData(rtSignalName, m_magnemetometerBuffer); } } } - vectorCollectionRTDataServer.sendData(); - -} - -void YarpRobotLoggerDevice::run() -{ - constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; - - // get the data - if (!m_robotSensorBridge->advance()) - { - log()->error("{} Could not advance sensor bridge.", logPrefix); - } - - const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); - - std::lock_guard lock(m_bufferManagerMutex); - - if (m_streamJointStates) - { - if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::positions"); - if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::velocities"); - if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::accelerations"); - if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::torques"); - } - - if (m_streamMotorStates) - { - if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::positions"); - if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::velocities"); - if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::accelerations"); - if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::currents"); - } - - if (m_streamMotorPWM) - { - if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::PWM"); - } - - if (m_streamPIDs) - { - if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) - m_bufferManager.push_back(m_jointSensorBuffer, time, "PIDs"); - } - - for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) - { - if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) - m_bufferManager.push_back(m_ftBuffer, time, "FTs::" + sensorName); - } - - for (const auto& sensorname : m_robotSensorBridge->getTemperatureSensorsList()) - { - if (m_robotSensorBridge->getTemperature(sensorname, m_ftTemperatureBuffer)) - m_bufferManager.push_back({m_ftTemperatureBuffer}, time, "temperatures::" + sensorname); - } - - for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) - { - if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) - m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); - } - - for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) - { - if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) - m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); - } - - for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) - { - if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) - m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); - } - - for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) - { - if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) - m_bufferManager.push_back(m_magnemetometerBuffer, time, "magnetometers::" + sensorName); - } - // an IMU contains a gyro accelerometer and an orientation sensor for (const auto& sensorName : m_robotSensorBridge->getIMUsList()) { @@ -1559,73 +1582,50 @@ void YarpRobotLoggerDevice::run() m_gyroBuffer, m_orientationBuffer); - m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); + signalName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + m_bufferManager.push_back(m_acceloremeterBuffer, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateData(rtSignalName, m_acceloremeterBuffer); - m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); + signalName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + m_bufferManager.push_back(m_gyroBuffer, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateData(rtSignalName, m_gyroBuffer); - m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); + signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + m_bufferManager.push_back(m_orientationBuffer, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + vectorCollectionRTDataServer.populateData(rtSignalName, m_orientationBuffer); } } - for (const auto& sensorName : m_robotSensorBridge->getCartesianWrenchesList()) + if (m_streamCartesianWrenches) { - if (m_robotSensorBridge->getCartesianWrench(sensorName, m_ftBuffer)) + for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { - m_bufferManager.push_back(m_ftBuffer, time, "cartesian_wrenches::" + sensorName); + if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) + { + signalName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; + m_bufferManager.push_back(m_ftBuffer, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; + vectorCollectionRTDataServer.populateData(rtSignalName, m_ftBuffer); + } } } - std::string signalFullName; for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); - externalSignalCollection = signal.client.readData(false); + BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection = signal.client.readData(false); if (externalSignalCollection != nullptr) { - if (!signal.dataArrived) - { - for (const auto& [key, vector] : externalSignalCollection->vectors) - { - signalFullName = signal.signalName + "::" + 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); - m_bufferManager.addChannel({signalFullName, {vector.size(), 1}}); - } else - { - // if the metadata is found we use it - m_bufferManager.addChannel({signalFullName, // - {vector.size(), 1}, - metadata->second}); - } - - signal.dataArrived = true; - } - } - for (const auto& [key, vector] : externalSignalCollection->vectors) { - signalFullName = signal.signalName + "::" + key; - m_bufferManager.push_back(vector, time, signalFullName); - } - } - } - - for (auto& [name, signal] : m_vectorSignals) - { - std::lock_guard lock(signal.mutex); - yarp::sig::Vector* vector = signal.port.read(false); - if (vector != nullptr) - { - if (!signal.dataArrived) - { - m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); - signal.dataArrived = true; + signalName = signal.signalName + std::string(TREE_DELIM) + key; + m_bufferManager.push_back(vector, time, signalName); + rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; + vectorCollectionRTDataServer.populateData(rtSignalName, vector); } } } @@ -1641,19 +1641,19 @@ void YarpRobotLoggerDevice::run() msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, std::to_string(time)); if (msg.isValid) { - signalFullName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName + signalName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName + "::p" + msg.processPID; // matlab does not support the character - as a key of a struct - findAndReplaceAll(signalFullName, "-", "_"); + findAndReplaceAll(signalName, "-", "_"); // if it is the first time this signal is seen by the device the channel is // added - if (m_textLogsStoredInManager.find(signalFullName) + if (m_textLogsStoredInManager.find(signalName) == m_textLogsStoredInManager.end()) { - m_bufferManager.addChannel({signalFullName, {1, 1}}); - m_textLogsStoredInManager.insert(signalFullName); + m_bufferManager.addChannel({signalName, {1, 1}}); + m_textLogsStoredInManager.insert(signalName); } } @@ -1664,7 +1664,7 @@ void YarpRobotLoggerDevice::run() } } - SendDataToLoggerVisualizer(); + vectorCollectionRTDataServer.sendData(); } bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, From 0ce15755280e00a05c9ee7e6e324fad3c07f4cd9 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Fri, 19 Jan 2024 15:03:51 +0100 Subject: [PATCH 31/83] Removed white space, fixed spelling mistake in comment --- .../src/YarpRobotLoggerDevice.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index d3a81b8536..ce6d6b44b8 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -690,7 +690,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // populate metadata for joint accelerations rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_ACCLERATIONS_NAME); vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - + // populate metadata for joint torques rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_TORQUES_NAME); vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); @@ -709,11 +709,11 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // populate metadata for motor state velocities rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_VELOCITIES_NAME); vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - + // populate metadata for motor state accelerations rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_ACCELERATIONS_NAME); vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - + // populate metadata for motor state currents rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); @@ -763,7 +763,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { - std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; ok = ok && m_bufferManager.addChannel({fullAccelerometerSensorName, {3, 1}, // @@ -801,7 +801,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // an IMU contains a gyro accelerometer and an orientation sensor for (const auto& sensorName : m_robotSensorBridge->getIMUsList()) { - std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; std::string fullGyroSensorName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; std::string fullOrientationsSensorName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; ok = ok @@ -835,7 +835,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) && m_bufferManager.addChannel({fullCartesianWrenchName, {6, 1}, // {CartesianWrenchNames[0], CartesianWrenchNames[1], CartesianWrenchNames[2], CartesianWrenchNames[3], CartesianWrenchNames[4], CartesianWrenchNames[5]}}); - + std::string rtFullCartesianWrenchName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; vectorCollectionRTDataServer.populateMetadata(rtFullCartesianWrenchName, {CartesianWrenchNames[0], CartesianWrenchNames[1], CartesianWrenchNames[2], CartesianWrenchNames[3], CartesianWrenchNames[4], CartesianWrenchNames[5]}); @@ -889,7 +889,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } } } - + for (auto& [name, signal] : m_vectorSignals) { std::lock_guard lock(signal.mutex); @@ -1576,7 +1576,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getIMUMeasurement(sensorName, m_analogSensorBuffer)) { - // it will return a tuple containing the Accelerometer, the gyro and the orientatio + // it will return a tuple containing the Accelerometer, the gyro and the orientation this->unpackIMU(m_analogSensorBuffer, m_acceloremeterBuffer, m_gyroBuffer, From cb4838ed5c98b34154fef683ec030b678fa85b65 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Fri, 19 Jan 2024 15:07:32 +0100 Subject: [PATCH 32/83] Updated formatting --- .../YarpRobotLoggerDeviceVariableTreeNames.h | 56 +++++++++---------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h index 9028e4c126..05f1adbac4 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h @@ -3,51 +3,51 @@ #define BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H -#define TREE_DELIM "::" +#define TREE_DELIM "::" -#define ROBOT_RT_ROOT_NAME "robot_realtime" +#define ROBOT_RT_ROOT_NAME "robot_realtime" -#define JOINT_STATE_POSITIONS_NAME "joints_state::positions" -#define JOINT_STATE_VELOCITIES_NAME "joints_state::velocities" -#define JOINT_STATE_ACCLERATIONS_NAME "joints_state::accelerations" -#define JOINT_STATE_TORQUES_NAME "joints_state::torques" +#define JOINT_STATE_POSITIONS_NAME "joints_state::positions" +#define JOINT_STATE_VELOCITIES_NAME "joints_state::velocities" +#define JOINT_STATE_ACCLERATIONS_NAME "joints_state::accelerations" +#define JOINT_STATE_TORQUES_NAME "joints_state::torques" -#define MOTOR_STATE_POSITIONS_NAME "motors_state::positions" -#define MOTOR_STATE_VELOCITIES_NAME "motors_state::velocities" +#define MOTOR_STATE_POSITIONS_NAME "motors_state::positions" +#define MOTOR_STATE_VELOCITIES_NAME "motors_state::velocities" #define MOTOR_STATE_ACCELERATIONS_NAME "motors_state::acclerations" -#define MOTOR_STATE_CURRENTS_NAME "motors_state::currents" -#define MOTOR_STATE_PWM_NAME "motors_state::PWM" +#define MOTOR_STATE_CURRENTS_NAME "motors_state::currents" +#define MOTOR_STATE_PWM_NAME "motors_state::PWM" -#define MOTOR_STATE_PIDS_NAME "PIDs" +#define MOTOR_STATE_PIDS_NAME "PIDs" -#define FTS_NAME "FTs" +#define FTS_NAME "FTs" -const std::string FTElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; +const std::string FTElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; -#define GYROS_NAME "gyros" -const std::string GyroElementNames[] = {"omega_x", "omega_y", "omega_z"}; +#define GYROS_NAME "gyros" +const std::string GyroElementNames[] = {"omega_x", "omega_y", "omega_z"}; -#define ACCELEROMETERS_NAME "accelerometers" -const std::string AccelerometerElementNames[] = {"a_x", "a_y", "a_z"}; +#define ACCELEROMETERS_NAME "accelerometers" +const std::string AccelerometerElementNames[] = {"a_x", "a_y", "a_z"}; -#define ORIENTATIONS_NAME "orientations" -const std::string OrientationElementNames[] = {"r", "p", "y"}; +#define ORIENTATIONS_NAME "orientations" +const std::string OrientationElementNames[] = {"r", "p", "y"}; -#define MAGNETOMETERS_NAME "magnetometers" -const std::string MagnetometerElementNames[] = {"mag_x", "mag_y", "mag_z"}; +#define MAGNETOMETERS_NAME "magnetometers" +const std::string MagnetometerElementNames[] = {"mag_x", "mag_y", "mag_z"}; -#define CARTESIAN_WRENCHES_NAME "cartesian_wrenches" -const std::string CartesianWrenchNames[] = {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}; +#define CARTESIAN_WRENCHES_NAME "cartesian_wrenches" +const std::string CartesianWrenchNames[] = {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}; -#define TEMPERATURE_NAME "temperatures" -const std::string TemperatureNames[] = {"temperature"}; +#define TEMPERATURE_NAME "temperatures" +const std::string TemperatureNames[] = {"temperature"}; -#define YARP_NAME "yarp_robot_name" +#define YARP_NAME "yarp_robot_name" -#define ROBOT_DESCRIPTON_LIST "description_list" +#define ROBOT_DESCRIPTON_LIST "description_list" -#define TIMESTAMPS_NAME "timestamps" +#define TIMESTAMPS_NAME "timestamps" #endif From 875e1906a38557be6760f5b7a05a116aae18008f Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Fri, 19 Jan 2024 15:34:39 +0100 Subject: [PATCH 33/83] Update YarpRobotLoggerDevice.cpp From 4815a616a97ca21398ed40e3c5ffeae78b49f48e Mon Sep 17 00:00:00 2001 From: Nick Date: Fri, 19 Jan 2024 15:38:47 +0100 Subject: [PATCH 34/83] remvoed white space --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index ce6d6b44b8..1151752069 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1591,7 +1591,7 @@ void YarpRobotLoggerDevice::run() m_bufferManager.push_back(m_gyroBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; vectorCollectionRTDataServer.populateData(rtSignalName, m_gyroBuffer); - + signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; m_bufferManager.push_back(m_orientationBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; From 1cde2aef0c642c040c3d323444e8e6de85433325 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Fri, 19 Jan 2024 15:42:51 +0100 Subject: [PATCH 35/83] Update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1027c62ad9..3961bf00c3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ All notable changes to this project are documented in this file. - 🤖 Add `ergoCubSN001` configuration files for the `balancing-position-control` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/776) - Implement `VectorsCollectionServer` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/776) - Implement `toString()` methods in `PlannedContact`, `ContactList`, `ContactPhase` and `ContactPhaseList` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/777) +- Added Vector Collection Server for publishing information for real-time users in the YARPRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/796) ### Changed - Restructure of the `CentroidalMPC` class in `ReducedModelControllers` component. Specifically, the `CentroidalMPC` now provides a contact phase list instead of indicating the next active contact. Additionally, users can now switch between `ipopt` and `sqpmethod` to solve the optimization problem. Furthermore, the update allows for setting the warm-start for the non-linear solver. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/766) From 3ee44813a2d3fff7918454e625dc4875650b3086 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 22 Jan 2024 10:59:12 +0100 Subject: [PATCH 36/83] Changed vector collection RT data server member name --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 2 +- .../src/YarpRobotLoggerDevice.cpp | 100 +++++++++--------- 2 files changed, 51 insertions(+), 51 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index fb9cebfb98..26c90d79ac 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -64,7 +64,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::unique_ptr m_robotSensorBridge; std::unique_ptr m_cameraBridge; - BipedalLocomotion::YarpUtilities::VectorsCollectionServer vectorCollectionRTDataServer; + BipedalLocomotion::YarpUtilities::VectorsCollectionServer m_vectorCollectionRTDataServer; template struct ExogenousSignal { diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 1151752069..5812187a14 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -652,23 +652,23 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // initialize the RT Logger Vector Collection Server auto loggerOption = std::make_shared(); loggerOption->setParameter("remote", "/testVectorCollections"); - if (!vectorCollectionRTDataServer.initialize(loggerOption)) + if (!m_vectorCollectionRTDataServer.initialize(loggerOption)) { log()->error("Failed to initalize the vectorsCollectionServer", logPrefix); return false; } char* tmp = std::getenv("YARP_ROBOT_NAME"); std::string metadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(YARP_NAME); - vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); + m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); metadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); - vectorCollectionRTDataServer.populateMetadata(metadataName, {TIMESTAMPS_NAME}); + m_vectorCollectionRTDataServer.populateMetadata(metadataName, {TIMESTAMPS_NAME}); const unsigned dofs = joints.size(); m_bufferManager.setDescriptionList(joints); std::string rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ROBOT_DESCRIPTON_LIST); - vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); // prepare the telemetry @@ -681,19 +681,19 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // populate metadata for joint positions rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_POSITIONS_NAME); - vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); // populate metadata for joint velocities rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_VELOCITIES_NAME); - vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); // populate metadata for joint accelerations rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_ACCLERATIONS_NAME); - vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); // populate metadata for joint torques rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_TORQUES_NAME); - vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); } if (m_streamMotorStates) { @@ -704,19 +704,19 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // populate metadata for motor state positions rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_POSITIONS_NAME); - vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); // populate metadata for motor state velocities rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_VELOCITIES_NAME); - vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); // populate metadata for motor state accelerations rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_ACCELERATIONS_NAME); - vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); // populate metadata for motor state currents rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); - vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); } if (m_streamMotorPWM) @@ -742,7 +742,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) std::string rtFullFTSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateMetadata(rtFullFTSensorName, {FTElementNames[0], FTElementNames[1], FTElementNames[2], + m_vectorCollectionRTDataServer.populateMetadata(rtFullFTSensorName, {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}); } } @@ -758,7 +758,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}}); std::string rtFullGyroSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateMetadata(rtFullGyroSensorName, {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}); + m_vectorCollectionRTDataServer.populateMetadata(rtFullGyroSensorName, {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) @@ -770,7 +770,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}}); std::string rtFullAccelerometerSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateMetadata(rtFullAccelerometerSensorName, {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}); + m_vectorCollectionRTDataServer.populateMetadata(rtFullAccelerometerSensorName, {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) @@ -783,7 +783,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}}); std::string rtFullOrientationSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateMetadata(rtFullOrientationSensorName, {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}); + m_vectorCollectionRTDataServer.populateMetadata(rtFullOrientationSensorName, {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) @@ -795,7 +795,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) {MagnetometerElementNames[0], MagnetometerElementNames[1], MagnetometerElementNames[2]}}); std::string rtFullMagnetometersSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateMetadata(rtFullMagnetometersSensorName, {MagnetometerElementNames[0], MagnetometerElementNames[1], MagnetometerElementNames[2]}); + m_vectorCollectionRTDataServer.populateMetadata(rtFullMagnetometersSensorName, {MagnetometerElementNames[0], MagnetometerElementNames[1], MagnetometerElementNames[2]}); } // an IMU contains a gyro accelerometer and an orientation sensor @@ -816,13 +816,13 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}}); std::string rtFullAccelerometerSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateMetadata(rtFullAccelerometerSensorName, {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}); + m_vectorCollectionRTDataServer.populateMetadata(rtFullAccelerometerSensorName, {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}); std::string rtFullGyroSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateMetadata(rtFullGyroSensorName, {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}); + m_vectorCollectionRTDataServer.populateMetadata(rtFullGyroSensorName, {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}); std::string rtFullOrientationSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateMetadata(rtFullOrientationSensorName, {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}); + m_vectorCollectionRTDataServer.populateMetadata(rtFullOrientationSensorName, {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}); } } @@ -837,7 +837,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) {CartesianWrenchNames[0], CartesianWrenchNames[1], CartesianWrenchNames[2], CartesianWrenchNames[3], CartesianWrenchNames[4], CartesianWrenchNames[5]}}); std::string rtFullCartesianWrenchName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; - vectorCollectionRTDataServer.populateMetadata(rtFullCartesianWrenchName, {CartesianWrenchNames[0], CartesianWrenchNames[1], CartesianWrenchNames[2], + m_vectorCollectionRTDataServer.populateMetadata(rtFullCartesianWrenchName, {CartesianWrenchNames[0], CartesianWrenchNames[1], CartesianWrenchNames[2], CartesianWrenchNames[3], CartesianWrenchNames[4], CartesianWrenchNames[5]}); } } @@ -852,7 +852,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) {1, 1}, // {TemperatureNames[0]}}); std::string rtFullTemperatureSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateMetadata(rtFullTemperatureSensorName, {TemperatureNames[0]}); + m_vectorCollectionRTDataServer.populateMetadata(rtFullTemperatureSensorName, {TemperatureNames[0]}); } } std::string signalName = ""; @@ -876,7 +876,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) m_bufferManager.addChannel({signalName, {vector.size(), 1}}); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; - vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); + m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); } else { // if the metadata is found we use it @@ -884,7 +884,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) {vector.size(), 1}, metadata->second}); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; - vectorCollectionRTDataServer.populateMetadata(rtSignalName, {metadata->second}); + m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {metadata->second}); } } } @@ -901,13 +901,13 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signal.signalName; - vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); + m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); signal.dataArrived = true; } } } - vectorCollectionRTDataServer.finalizeMetadata(); + m_vectorCollectionRTDataServer.finalizeMetadata(); // resize the temporary vectors m_jointSensorBuffer.resize(dofs); @@ -1394,7 +1394,7 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit void YarpRobotLoggerDevice::run() { constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; - vectorCollectionRTDataServer.clearData(); + m_vectorCollectionRTDataServer.clearData(); // get the data if (!m_robotSensorBridge->advance()) @@ -1413,7 +1413,7 @@ void YarpRobotLoggerDevice::run() std::string rtSignalName = ""; rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, timeData); + m_vectorCollectionRTDataServer.populateData(rtSignalName, timeData); if (m_streamJointStates) { @@ -1421,25 +1421,25 @@ void YarpRobotLoggerDevice::run() { m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_POSITIONS_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_POSITIONS_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) { m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_VELOCITIES_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_VELOCITIES_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) { m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_ACCLERATIONS_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_ACCLERATIONS_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_TORQUES_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_TORQUES_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } } @@ -1449,25 +1449,25 @@ void YarpRobotLoggerDevice::run() { m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_POSITIONS_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_POSITIONS_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_VELOCITIES_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_VELOCITIES_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_ACCELERATIONS_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_ACCELERATIONS_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_CURRENTS_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } } @@ -1477,7 +1477,7 @@ void YarpRobotLoggerDevice::run() { m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_PWM_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } } @@ -1487,7 +1487,7 @@ void YarpRobotLoggerDevice::run() { m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_PIDS_NAME); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_PIDS_NAME); - vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); } } @@ -1500,7 +1500,7 @@ void YarpRobotLoggerDevice::run() signalName = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; m_bufferManager.push_back(m_ftBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateData(rtSignalName, m_ftBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_ftBuffer); } } } @@ -1517,7 +1517,7 @@ void YarpRobotLoggerDevice::run() Eigen::Matrix temperatureData; temperatureData << m_ftTemperatureBuffer; rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateData(rtSignalName, temperatureData); + m_vectorCollectionRTDataServer.populateData(rtSignalName, temperatureData); } } } @@ -1531,7 +1531,7 @@ void YarpRobotLoggerDevice::run() signalName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; m_bufferManager.push_back(m_gyroBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateData(rtSignalName, m_gyroBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_gyroBuffer); } } @@ -1543,7 +1543,7 @@ void YarpRobotLoggerDevice::run() signalName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; m_bufferManager.push_back(m_acceloremeterBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateData(rtSignalName, m_acceloremeterBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_acceloremeterBuffer); } } @@ -1555,7 +1555,7 @@ void YarpRobotLoggerDevice::run() signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; m_bufferManager.push_back(m_orientationBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateData(rtSignalName, m_orientationBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_orientationBuffer); } } @@ -1566,7 +1566,7 @@ void YarpRobotLoggerDevice::run() signalName = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; m_bufferManager.push_back(m_magnemetometerBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateData(rtSignalName, m_magnemetometerBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_magnemetometerBuffer); } } } @@ -1585,17 +1585,17 @@ void YarpRobotLoggerDevice::run() signalName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; m_bufferManager.push_back(m_acceloremeterBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateData(rtSignalName, m_acceloremeterBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_acceloremeterBuffer); signalName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; m_bufferManager.push_back(m_gyroBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateData(rtSignalName, m_gyroBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_gyroBuffer); signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; m_bufferManager.push_back(m_orientationBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - vectorCollectionRTDataServer.populateData(rtSignalName, m_orientationBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_orientationBuffer); } } @@ -1608,7 +1608,7 @@ void YarpRobotLoggerDevice::run() signalName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; m_bufferManager.push_back(m_ftBuffer, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; - vectorCollectionRTDataServer.populateData(rtSignalName, m_ftBuffer); + m_vectorCollectionRTDataServer.populateData(rtSignalName, m_ftBuffer); } } } @@ -1625,7 +1625,7 @@ void YarpRobotLoggerDevice::run() signalName = signal.signalName + std::string(TREE_DELIM) + key; m_bufferManager.push_back(vector, time, signalName); rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; - vectorCollectionRTDataServer.populateData(rtSignalName, vector); + m_vectorCollectionRTDataServer.populateData(rtSignalName, vector); } } } @@ -1664,7 +1664,7 @@ void YarpRobotLoggerDevice::run() } } - vectorCollectionRTDataServer.sendData(); + m_vectorCollectionRTDataServer.sendData(); } bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, From 12f164926d7e46f74882d66b653674543accda12 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 22 Jan 2024 12:11:51 +0100 Subject: [PATCH 37/83] Added functions for processing metadata --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 2 + .../src/YarpRobotLoggerDevice.cpp | 150 +++++------------- 2 files changed, 38 insertions(+), 114 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 26c90d79ac..67f9f7d13b 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -177,6 +177,8 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void lookForNewLogs(); void lookForExogenousSignals(); + bool addJointRelatedChannelAndMetadata(const std::string& name, const std::vector& jointNames); + bool addSensorBridgeRelatedChannelAndMetadata(const std::string& nameKey, const std::vector& metadata); bool hasSubstring(const std::string& str, const std::vector& substrings) const; void recordVideo(const std::string& cameraName, VideoWriter& writer); void unpackIMU(Eigen::Ref signal, diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 5812187a14..df9cc0b312 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -608,6 +608,22 @@ bool YarpRobotLoggerDevice::setupRobotCameraBridge( return true; } +bool YarpRobotLoggerDevice::addJointRelatedChannelAndMetadata(const std::string& name, const std::vector& jointNames) +{ + bool ok = m_bufferManager.addChannel({name, {jointNames.size(), 1}, jointNames}); + std::string rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + name; + ok &= m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, jointNames); + return ok; +} + +bool YarpRobotLoggerDevice::addSensorBridgeRelatedChannelAndMetadata(const std::string& nameKey, const std::vector& metadataNames) +{ + bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames}); + std::string rtNameKey = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + nameKey; + ok &= m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames); + return ok; +} + bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { constexpr auto logPrefix = "[YarpRobotLoggerDevice::attachAll]"; @@ -674,59 +690,27 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // prepare the telemetry if (m_streamJointStates) { - ok = ok && m_bufferManager.addChannel({JOINT_STATE_POSITIONS_NAME, {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({JOINT_STATE_VELOCITIES_NAME, {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({JOINT_STATE_ACCLERATIONS_NAME, {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({JOINT_STATE_TORQUES_NAME, {dofs, 1}, joints}); - - // populate metadata for joint positions - rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_POSITIONS_NAME); - m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - - // populate metadata for joint velocities - rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_VELOCITIES_NAME); - m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - - // populate metadata for joint accelerations - rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_ACCLERATIONS_NAME); - m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - - // populate metadata for joint torques - rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_TORQUES_NAME); - m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + ok &= addJointRelatedChannelAndMetadata(JOINT_STATE_POSITIONS_NAME, joints); + ok &= addJointRelatedChannelAndMetadata(JOINT_STATE_VELOCITIES_NAME, joints); + ok &= addJointRelatedChannelAndMetadata(JOINT_STATE_ACCLERATIONS_NAME, joints); + ok &= addJointRelatedChannelAndMetadata(JOINT_STATE_TORQUES_NAME, joints); } if (m_streamMotorStates) { - ok = ok && m_bufferManager.addChannel({MOTOR_STATE_POSITIONS_NAME, {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({MOTOR_STATE_VELOCITIES_NAME, {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({MOTOR_STATE_ACCELERATIONS_NAME, {dofs, 1}, joints}); - ok = ok && m_bufferManager.addChannel({MOTOR_STATE_CURRENTS_NAME, {dofs, 1}, joints}); - - // populate metadata for motor state positions - rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_POSITIONS_NAME); - m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - - // populate metadata for motor state velocities - rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_VELOCITIES_NAME); - m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - - // populate metadata for motor state accelerations - rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_ACCELERATIONS_NAME); - m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - - // populate metadata for motor state currents - rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); - m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_POSITIONS_NAME, joints); + ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_VELOCITIES_NAME, joints); + ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_ACCELERATIONS_NAME, joints); + ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_CURRENTS_NAME, joints); } if (m_streamMotorPWM) { - ok = ok && m_bufferManager.addChannel({MOTOR_STATE_PWM_NAME, {dofs, 1}, joints}); + ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_PWM_NAME, joints); } if (m_streamPIDs) { - ok = ok && m_bufferManager.addChannel({MOTOR_STATE_PIDS_NAME, {dofs, 1}, joints}); + ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_PIDS_NAME, joints); } if (m_streamFTSensors) @@ -734,16 +718,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { std::string fullFTSensorName = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; - ok = ok - && m_bufferManager.addChannel({fullFTSensorName, - {6, 1}, // - {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}}); - - std::string rtFullFTSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) - + std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; - - m_vectorCollectionRTDataServer.populateMetadata(rtFullFTSensorName, {FTElementNames[0], FTElementNames[1], FTElementNames[2], - FTElementNames[3], FTElementNames[4], FTElementNames[5]}); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullFTSensorName, FTElementNames); } } @@ -752,50 +727,25 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { std::string fullGyroSensorName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - ok = ok - && m_bufferManager.addChannel({fullGyroSensorName, - {3, 1}, // - {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}}); - - std::string rtFullGyroSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateMetadata(rtFullGyroSensorName, {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullGyroSensorName, GyroElementNames); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - ok = ok - && m_bufferManager.addChannel({fullAccelerometerSensorName, - {3, 1}, // - {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}}); - - std::string rtFullAccelerometerSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateMetadata(rtFullAccelerometerSensorName, {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { std::string fullOrientationsSensorName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - - ok = ok - && m_bufferManager.addChannel({fullOrientationsSensorName, - {3, 1}, // - {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}}); - - std::string rtFullOrientationSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateMetadata(rtFullOrientationSensorName, {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { std::string fullMagnetometersSensorName = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - ok = ok - && m_bufferManager.addChannel({fullMagnetometersSensorName, - {3, 1}, // - {MagnetometerElementNames[0], MagnetometerElementNames[1], MagnetometerElementNames[2]}}); - - std::string rtFullMagnetometersSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateMetadata(rtFullMagnetometersSensorName, {MagnetometerElementNames[0], MagnetometerElementNames[1], MagnetometerElementNames[2]}); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullMagnetometersSensorName, MagnetometerElementNames); } // an IMU contains a gyro accelerometer and an orientation sensor @@ -804,25 +754,9 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; std::string fullGyroSensorName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; std::string fullOrientationsSensorName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - ok = ok - && m_bufferManager.addChannel({fullAccelerometerSensorName, - {3, 1}, // - {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}}) - && m_bufferManager.addChannel({fullGyroSensorName, - {3, 1}, // - {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}}) - && m_bufferManager.addChannel({fullOrientationsSensorName, - {3, 1}, // - {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}}); - - std::string rtFullAccelerometerSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateMetadata(rtFullAccelerometerSensorName, {AccelerometerElementNames[0], AccelerometerElementNames[1], AccelerometerElementNames[2]}); - - std::string rtFullGyroSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateMetadata(rtFullGyroSensorName, {GyroElementNames[0], GyroElementNames[1], GyroElementNames[2]}); - - std::string rtFullOrientationSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateMetadata(rtFullOrientationSensorName, {OrientationElementNames[0], OrientationElementNames[1], OrientationElementNames[2]}); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullGyroSensorName, GyroElementNames); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); } } @@ -831,14 +765,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { std::string fullCartesianWrenchName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; - ok = ok - && m_bufferManager.addChannel({fullCartesianWrenchName, - {6, 1}, // - {CartesianWrenchNames[0], CartesianWrenchNames[1], CartesianWrenchNames[2], CartesianWrenchNames[3], CartesianWrenchNames[4], CartesianWrenchNames[5]}}); - - std::string rtFullCartesianWrenchName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; - m_vectorCollectionRTDataServer.populateMetadata(rtFullCartesianWrenchName, {CartesianWrenchNames[0], CartesianWrenchNames[1], CartesianWrenchNames[2], - CartesianWrenchNames[3], CartesianWrenchNames[4], CartesianWrenchNames[5]}); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullCartesianWrenchName, CartesianWrenchNames); } } @@ -847,12 +774,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { std::string fullTemperatureSensorName = std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; - ok = ok - && m_bufferManager.addChannel({fullTemperatureSensorName, - {1, 1}, // - {TemperatureNames[0]}}); - std::string rtFullTemperatureSensorName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateMetadata(rtFullTemperatureSensorName, {TemperatureNames[0]}); + ok &= addSensorBridgeRelatedChannelAndMetadata(fullTemperatureSensorName, TemperatureNames); } } std::string signalName = ""; From 3fda2d57caa86047d699edf34e5ec0598b5c87df Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 22 Jan 2024 12:12:22 +0100 Subject: [PATCH 38/83] Optimized the constants a bit --- .../YarpRobotLoggerDeviceVariableTreeNames.h | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h index 05f1adbac4..5dfb67e7d4 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h @@ -1,9 +1,10 @@ #ifndef BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H #include +#include #define BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H -#define TREE_DELIM "::" +constexpr std::string_view TREE_DELIM {"::"}; #define ROBOT_RT_ROOT_NAME "robot_realtime" @@ -23,25 +24,25 @@ #define FTS_NAME "FTs" -const std::string FTElementNames[] = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; +const std::vector FTElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; #define GYROS_NAME "gyros" -const std::string GyroElementNames[] = {"omega_x", "omega_y", "omega_z"}; +const std::vector GyroElementNames = {"omega_x", "omega_y", "omega_z"}; #define ACCELEROMETERS_NAME "accelerometers" -const std::string AccelerometerElementNames[] = {"a_x", "a_y", "a_z"}; +const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; #define ORIENTATIONS_NAME "orientations" -const std::string OrientationElementNames[] = {"r", "p", "y"}; +const std::vector OrientationElementNames = {"r", "p", "y"}; #define MAGNETOMETERS_NAME "magnetometers" -const std::string MagnetometerElementNames[] = {"mag_x", "mag_y", "mag_z"}; +const std::vector MagnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; #define CARTESIAN_WRENCHES_NAME "cartesian_wrenches" -const std::string CartesianWrenchNames[] = {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}; +const std::vector CartesianWrenchNames = {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}; #define TEMPERATURE_NAME "temperatures" -const std::string TemperatureNames[] = {"temperature"}; +const std::vector TemperatureNames = {"temperature"}; #define YARP_NAME "yarp_robot_name" From 20d7b65966c8131ab335c9834b212c9f548cf982 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 22 Jan 2024 12:29:22 +0100 Subject: [PATCH 39/83] Removed identical functions --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 7 ++- .../src/YarpRobotLoggerDevice.cpp | 50 ++++++++----------- 2 files changed, 26 insertions(+), 31 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 67f9f7d13b..7d0bde6a0c 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -177,8 +177,11 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void lookForNewLogs(); void lookForExogenousSignals(); - bool addJointRelatedChannelAndMetadata(const std::string& name, const std::vector& jointNames); - bool addSensorBridgeRelatedChannelAndMetadata(const std::string& nameKey, const std::vector& metadata); + + + bool addChannelAndMetadata(const std::string& nameKey, const std::vector& metadata); + + bool hasSubstring(const std::string& str, const std::vector& substrings) const; void recordVideo(const std::string& cameraName, VideoWriter& writer); void unpackIMU(Eigen::Ref signal, diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index c8fcce5734..e1bd508f7e 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -608,15 +608,7 @@ bool YarpRobotLoggerDevice::setupRobotCameraBridge( return true; } -bool YarpRobotLoggerDevice::addJointRelatedChannelAndMetadata(const std::string& name, const std::vector& jointNames) -{ - bool ok = m_bufferManager.addChannel({name, {jointNames.size(), 1}, jointNames}); - std::string rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + name; - ok &= m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, jointNames); - return ok; -} - -bool YarpRobotLoggerDevice::addSensorBridgeRelatedChannelAndMetadata(const std::string& nameKey, const std::vector& metadataNames) +bool YarpRobotLoggerDevice::addChannelAndMetadata(const std::string& nameKey, const std::vector& metadataNames) { bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames}); std::string rtNameKey = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + nameKey; @@ -690,27 +682,27 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // prepare the telemetry if (m_streamJointStates) { - ok &= addJointRelatedChannelAndMetadata(JOINT_STATE_POSITIONS_NAME, joints); - ok &= addJointRelatedChannelAndMetadata(JOINT_STATE_VELOCITIES_NAME, joints); - ok &= addJointRelatedChannelAndMetadata(JOINT_STATE_ACCLERATIONS_NAME, joints); - ok &= addJointRelatedChannelAndMetadata(JOINT_STATE_TORQUES_NAME, joints); + ok &= addChannelAndMetadata(JOINT_STATE_POSITIONS_NAME, joints); + ok &= addChannelAndMetadata(JOINT_STATE_VELOCITIES_NAME, joints); + ok &= addChannelAndMetadata(JOINT_STATE_ACCLERATIONS_NAME, joints); + ok &= addChannelAndMetadata(JOINT_STATE_TORQUES_NAME, joints); } if (m_streamMotorStates) { - ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_POSITIONS_NAME, joints); - ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_VELOCITIES_NAME, joints); - ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_ACCELERATIONS_NAME, joints); - ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_CURRENTS_NAME, joints); + ok &= addChannelAndMetadata(MOTOR_STATE_POSITIONS_NAME, joints); + ok &= addChannelAndMetadata(MOTOR_STATE_VELOCITIES_NAME, joints); + ok &= addChannelAndMetadata(MOTOR_STATE_ACCELERATIONS_NAME, joints); + ok &= addChannelAndMetadata(MOTOR_STATE_CURRENTS_NAME, joints); } if (m_streamMotorPWM) { - ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_PWM_NAME, joints); + ok &= addChannelAndMetadata(MOTOR_STATE_PWM_NAME, joints); } if (m_streamPIDs) { - ok &= addJointRelatedChannelAndMetadata(MOTOR_STATE_PIDS_NAME, joints); + ok &= addChannelAndMetadata(MOTOR_STATE_PIDS_NAME, joints); } if (m_streamFTSensors) @@ -718,7 +710,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { std::string fullFTSensorName = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; - ok &= addSensorBridgeRelatedChannelAndMetadata(fullFTSensorName, FTElementNames); + ok &= addChannelAndMetadata(fullFTSensorName, FTElementNames); } } @@ -727,25 +719,25 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { std::string fullGyroSensorName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - ok &= addSensorBridgeRelatedChannelAndMetadata(fullGyroSensorName, GyroElementNames); + ok &= addChannelAndMetadata(fullGyroSensorName, GyroElementNames); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - ok &= addSensorBridgeRelatedChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); + ok &= addChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { std::string fullOrientationsSensorName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - ok &= addSensorBridgeRelatedChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); + ok &= addChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { std::string fullMagnetometersSensorName = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - ok &= addSensorBridgeRelatedChannelAndMetadata(fullMagnetometersSensorName, MagnetometerElementNames); + ok &= addChannelAndMetadata(fullMagnetometersSensorName, MagnetometerElementNames); } // an IMU contains a gyro accelerometer and an orientation sensor @@ -754,9 +746,9 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; std::string fullGyroSensorName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; std::string fullOrientationsSensorName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - ok &= addSensorBridgeRelatedChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); - ok &= addSensorBridgeRelatedChannelAndMetadata(fullGyroSensorName, GyroElementNames); - ok &= addSensorBridgeRelatedChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); + ok &= addChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); + ok &= addChannelAndMetadata(fullGyroSensorName, GyroElementNames); + ok &= addChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); } } @@ -765,7 +757,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { std::string fullCartesianWrenchName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; - ok &= addSensorBridgeRelatedChannelAndMetadata(fullCartesianWrenchName, CartesianWrenchNames); + ok &= addChannelAndMetadata(fullCartesianWrenchName, CartesianWrenchNames); } } @@ -774,7 +766,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { std::string fullTemperatureSensorName = std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; - ok &= addSensorBridgeRelatedChannelAndMetadata(fullTemperatureSensorName, TemperatureNames); + ok &= addChannelAndMetadata(fullTemperatureSensorName, TemperatureNames); } } std::string signalName = ""; From 834d6186c3c2cf35457bbae386c0d8d627ff69e9 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 22 Jan 2024 14:27:03 +0100 Subject: [PATCH 40/83] Added function for storing and sending data --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 2 +- .../src/YarpRobotLoggerDevice.cpp | 88 ++++++------------- 2 files changed, 29 insertions(+), 61 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 7d0bde6a0c..d90c674951 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, bool addChannelAndMetadata(const std::string& nameKey, const std::vector& metadata); - + void storeAndSendLoggingData(const std::string& name, const Eigen::VectorXd& data, const double time); bool hasSubstring(const std::string& str, const std::vector& substrings) const; void recordVideo(const std::string& cameraName, VideoWriter& writer); diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index e1bd508f7e..481bfd1a78 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -616,6 +616,13 @@ bool YarpRobotLoggerDevice::addChannelAndMetadata(const std::string& nameKey, co return ok; } +void YarpRobotLoggerDevice::storeAndSendLoggingData(const std::string& name, const Eigen::VectorXd& data, const double time) +{ + m_bufferManager.push_back(data, time, name); + std::string rtName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + name; + m_vectorCollectionRTDataServer.populateData(rtName, data); +} + bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { constexpr auto logPrefix = "[YarpRobotLoggerDevice::attachAll]"; @@ -1336,27 +1343,19 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_POSITIONS_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_POSITIONS_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(JOINT_STATE_POSITIONS_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_VELOCITIES_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_VELOCITIES_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(JOINT_STATE_VELOCITIES_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_ACCLERATIONS_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_ACCLERATIONS_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(JOINT_STATE_ACCLERATIONS_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, JOINT_STATE_TORQUES_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(JOINT_STATE_TORQUES_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(JOINT_STATE_TORQUES_NAME, m_jointSensorBuffer, time); } } @@ -1364,27 +1363,19 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_POSITIONS_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_POSITIONS_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(MOTOR_STATE_POSITIONS_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_VELOCITIES_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_VELOCITIES_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(MOTOR_STATE_VELOCITIES_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_ACCELERATIONS_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_ACCELERATIONS_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(MOTOR_STATE_ACCELERATIONS_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_CURRENTS_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(MOTOR_STATE_CURRENTS_NAME, m_jointSensorBuffer, time); } } @@ -1392,9 +1383,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_PWM_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_CURRENTS_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(MOTOR_STATE_PWM_NAME, m_jointSensorBuffer, time); } } @@ -1402,9 +1391,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, MOTOR_STATE_PIDS_NAME); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MOTOR_STATE_PIDS_NAME); - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_jointSensorBuffer); + storeAndSendLoggingData(MOTOR_STATE_PIDS_NAME, m_jointSensorBuffer, time); } } @@ -1415,9 +1402,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { signalName = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; - m_bufferManager.push_back(m_ftBuffer, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_ftBuffer); + storeAndSendLoggingData(signalName, m_ftBuffer, time); } } } @@ -1429,12 +1414,10 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) { signalName = std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; - m_bufferManager.push_back({m_ftTemperatureBuffer}, time, signalName); Eigen::Matrix temperatureData; temperatureData << m_ftTemperatureBuffer; - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, temperatureData); + storeAndSendLoggingData(signalName, temperatureData, time); } } } @@ -1446,9 +1429,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { signalName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - m_bufferManager.push_back(m_gyroBuffer, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_gyroBuffer); + storeAndSendLoggingData(signalName, m_gyroBuffer, time); } } @@ -1458,9 +1439,7 @@ void YarpRobotLoggerDevice::run() if(m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) { signalName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - m_bufferManager.push_back(m_acceloremeterBuffer, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_acceloremeterBuffer); + storeAndSendLoggingData(signalName, m_acceloremeterBuffer, time); } } @@ -1470,9 +1449,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) { signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - m_bufferManager.push_back(m_orientationBuffer, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_orientationBuffer); + storeAndSendLoggingData(signalName, m_orientationBuffer, time); } } @@ -1481,9 +1458,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { signalName = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - m_bufferManager.push_back(m_magnemetometerBuffer, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_magnemetometerBuffer); + storeAndSendLoggingData(signalName, m_magnemetometerBuffer, time); } } } @@ -1500,19 +1475,14 @@ void YarpRobotLoggerDevice::run() m_orientationBuffer); signalName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - m_bufferManager.push_back(m_acceloremeterBuffer, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_acceloremeterBuffer); + storeAndSendLoggingData(signalName, m_acceloremeterBuffer, time); signalName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - m_bufferManager.push_back(m_gyroBuffer, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_gyroBuffer); + storeAndSendLoggingData(signalName, m_gyroBuffer, time); signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - m_bufferManager.push_back(m_orientationBuffer, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_orientationBuffer); + storeAndSendLoggingData(signalName, m_orientationBuffer, time); + } } @@ -1523,9 +1493,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) { signalName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; - m_bufferManager.push_back(m_ftBuffer, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, m_ftBuffer); + storeAndSendLoggingData(signalName, m_ftBuffer, time); } } } From bf7fe13ca037a598119aaab783e0961903dd546f Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 22 Jan 2024 14:28:06 +0100 Subject: [PATCH 41/83] Fixed up constants file --- .../YarpRobotLoggerDeviceVariableTreeNames.h | 60 +++++++++---------- 1 file changed, 30 insertions(+), 30 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h index 5dfb67e7d4..1efc16bff0 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h @@ -1,54 +1,54 @@ #ifndef BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H -#include +#include #include #define BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H -constexpr std::string_view TREE_DELIM {"::"}; +#define TREE_DELIM "::" -#define ROBOT_RT_ROOT_NAME "robot_realtime" +#define ROBOT_RT_ROOT_NAME "robot_realtime" -#define JOINT_STATE_POSITIONS_NAME "joints_state::positions" -#define JOINT_STATE_VELOCITIES_NAME "joints_state::velocities" -#define JOINT_STATE_ACCLERATIONS_NAME "joints_state::accelerations" -#define JOINT_STATE_TORQUES_NAME "joints_state::torques" +#define JOINT_STATE_POSITIONS_NAME "joints_state::positions" +#define JOINT_STATE_VELOCITIES_NAME "joints_state::velocities" +#define JOINT_STATE_ACCLERATIONS_NAME "joints_state::accelerations" +#define JOINT_STATE_TORQUES_NAME "joints_state::torques" -#define MOTOR_STATE_POSITIONS_NAME "motors_state::positions" -#define MOTOR_STATE_VELOCITIES_NAME "motors_state::velocities" -#define MOTOR_STATE_ACCELERATIONS_NAME "motors_state::acclerations" -#define MOTOR_STATE_CURRENTS_NAME "motors_state::currents" -#define MOTOR_STATE_PWM_NAME "motors_state::PWM" +#define MOTOR_STATE_POSITIONS_NAME "motors_state::positions" +#define MOTOR_STATE_VELOCITIES_NAME "motors_state::velocities" +#define MOTOR_STATE_ACCELERATIONS_NAME "motors_state::acclerations" +#define MOTOR_STATE_CURRENTS_NAME "motors_state::currents" +#define MOTOR_STATE_PWM_NAME "motors_state::PWM" -#define MOTOR_STATE_PIDS_NAME "PIDs" +#define MOTOR_STATE_PIDS_NAME "PIDs" -#define FTS_NAME "FTs" +#define FTS_NAME "FTs" -const std::vector FTElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; +const std::vector FTElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; -#define GYROS_NAME "gyros" -const std::vector GyroElementNames = {"omega_x", "omega_y", "omega_z"}; +#define GYROS_NAME "gyros" +const std::vector GyroElementNames = {"omega_x", "omega_y", "omega_z"}; -#define ACCELEROMETERS_NAME "accelerometers" -const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; +#define ACCELEROMETERS_NAME "accelerometers" +const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; -#define ORIENTATIONS_NAME "orientations" -const std::vector OrientationElementNames = {"r", "p", "y"}; +#define ORIENTATIONS_NAME "orientations" +const std::vector OrientationElementNames = {"r", "p", "y"}; -#define MAGNETOMETERS_NAME "magnetometers" -const std::vector MagnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; +#define MAGNETOMETERS_NAME "magnetometers" +const std::vector MagnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; -#define CARTESIAN_WRENCHES_NAME "cartesian_wrenches" -const std::vector CartesianWrenchNames = {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}; +#define CARTESIAN_WRENCHES_NAME "cartesian_wrenches" +const std::vector CartesianWrenchNames = {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}; -#define TEMPERATURE_NAME "temperatures" -const std::vector TemperatureNames = {"temperature"}; +#define TEMPERATURE_NAME "temperatures" +const std::vector TemperatureNames = {"temperature"}; -#define YARP_NAME "yarp_robot_name" +#define YARP_NAME "yarp_robot_name" -#define ROBOT_DESCRIPTON_LIST "description_list" +#define ROBOT_DESCRIPTON_LIST "description_list" -#define TIMESTAMPS_NAME "timestamps" +#define TIMESTAMPS_NAME "timestamps" #endif From 5f62eeab579849a554b67ead618f1b1f6909518c Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 22 Jan 2024 14:54:56 +0100 Subject: [PATCH 42/83] Added mutex for text logging port --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 1 + devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index d90c674951..618d7580fa 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -173,6 +173,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::mutex m_bufferManagerMutex; + std::mutex m_textLoggingPortMutex; robometry::BufferManager m_bufferManager; void lookForNewLogs(); diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 481bfd1a78..1e230f36e5 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1164,6 +1164,7 @@ void YarpRobotLoggerDevice::lookForNewLogs() // check for new messages yarp::profiler::NetworkProfiler::getPortsList(yarpPorts); + m_textLoggingPortMutex.lock(); for (const auto& port : yarpPorts) { // check if the port has not be already connected if exits, its resposive @@ -1178,6 +1179,7 @@ void YarpRobotLoggerDevice::lookForNewLogs() yarp::os::Network::connect(port.name, m_textLoggingPortName, "udp"); } } + m_textLoggingPortMutex.unlock(); // release the CPU BipedalLocomotion::clock().yield(); @@ -1515,6 +1517,8 @@ void YarpRobotLoggerDevice::run() } } + + m_textLoggingPortMutex.lock(); int bufferportSize = m_textLoggingPort.getPendingReads(); BipedalLocomotion::TextLoggingEntry msg; @@ -1548,6 +1552,7 @@ void YarpRobotLoggerDevice::run() break; } } + m_textLoggingPortMutex.unlock(); m_vectorCollectionRTDataServer.sendData(); } From 8d3d48880fb2b4a0eecd8ed462cc2fb38838b80e Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 22 Jan 2024 16:17:56 +0100 Subject: [PATCH 43/83] Moved realtime port data to be read from config file --- .../src/YarpRobotLoggerDevice.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 1e230f36e5..6645001108 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -126,7 +126,11 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) constexpr auto logPrefix = "[YarpRobotLoggerDevice::open]"; auto params = std::make_shared(config); - + if (!m_vectorCollectionRTDataServer.initialize(params)) + { + log()->error("Failed to initalize the vectorsCollectionServer", logPrefix); + return false; + } double devicePeriod{0.01}; if (params->getParameter("sampling_period_in_s", devicePeriod)) { @@ -664,14 +668,6 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) return false; } - // initialize the RT Logger Vector Collection Server - auto loggerOption = std::make_shared(); - loggerOption->setParameter("remote", "/testVectorCollections"); - if (!m_vectorCollectionRTDataServer.initialize(loggerOption)) - { - log()->error("Failed to initalize the vectorsCollectionServer", logPrefix); - return false; - } char* tmp = std::getenv("YARP_ROBOT_NAME"); std::string metadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(YARP_NAME); m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); From 21bdbdbb729890dbd5f758b741f83153aa91cbf3 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 22 Jan 2024 16:29:19 +0100 Subject: [PATCH 44/83] Applied the clang format --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 12 +- .../YarpRobotLoggerDeviceVariableTreeNames.h | 68 +++++------ .../YarpTextLoggingUtilities.h | 3 +- .../src/YarpRobotLoggerDevice.cpp | 111 +++++++++++------- 4 files changed, 106 insertions(+), 88 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 618d7580fa..bd1dbe53e7 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -14,7 +14,6 @@ #include #include - #include #include @@ -102,13 +101,11 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool connect(); void disconnect(); - }; std::unordered_map m_vectorsCollectionSignals; std::unordered_map> m_vectorSignals; - std::unordered_set m_exogenousPortsStoredInManager; std::atomic m_lookForNewExogenousSignalIsRunning{false}; std::thread m_lookForNewExogenousSignalThread; @@ -171,7 +168,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::vector m_textLoggingSubnames; std::vector m_codeStatusCmdPrefixes; - std::mutex m_bufferManagerMutex; std::mutex m_textLoggingPortMutex; robometry::BufferManager m_bufferManager; @@ -179,9 +175,11 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void lookForNewLogs(); void lookForExogenousSignals(); - - bool addChannelAndMetadata(const std::string& nameKey, const std::vector& metadata); - void storeAndSendLoggingData(const std::string& name, const Eigen::VectorXd& data, const double time); + bool + addChannelAndMetadata(const std::string& nameKey, const std::vector& metadata); + void storeAndSendLoggingData(const std::string& name, + const Eigen::VectorXd& data, + const double time); bool hasSubstring(const std::string& str, const std::vector& substrings) const; void recordVideo(const std::string& cameraName, VideoWriter& writer); diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h index 1efc16bff0..5aa7e7faf6 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h @@ -4,54 +4,54 @@ #define BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H -#define TREE_DELIM "::" +#define TREE_DELIM "::" -#define ROBOT_RT_ROOT_NAME "robot_realtime" +#define ROBOT_RT_ROOT_NAME "robot_realtime" +#define JOINT_STATE_POSITIONS_NAME "joints_state::positions" +#define JOINT_STATE_VELOCITIES_NAME "joints_state::velocities" +#define JOINT_STATE_ACCLERATIONS_NAME "joints_state::accelerations" +#define JOINT_STATE_TORQUES_NAME "joints_state::torques" -#define JOINT_STATE_POSITIONS_NAME "joints_state::positions" -#define JOINT_STATE_VELOCITIES_NAME "joints_state::velocities" -#define JOINT_STATE_ACCLERATIONS_NAME "joints_state::accelerations" -#define JOINT_STATE_TORQUES_NAME "joints_state::torques" +#define MOTOR_STATE_POSITIONS_NAME "motors_state::positions" +#define MOTOR_STATE_VELOCITIES_NAME "motors_state::velocities" +#define MOTOR_STATE_ACCELERATIONS_NAME "motors_state::acclerations" +#define MOTOR_STATE_CURRENTS_NAME "motors_state::currents" +#define MOTOR_STATE_PWM_NAME "motors_state::PWM" -#define MOTOR_STATE_POSITIONS_NAME "motors_state::positions" -#define MOTOR_STATE_VELOCITIES_NAME "motors_state::velocities" -#define MOTOR_STATE_ACCELERATIONS_NAME "motors_state::acclerations" -#define MOTOR_STATE_CURRENTS_NAME "motors_state::currents" -#define MOTOR_STATE_PWM_NAME "motors_state::PWM" +#define MOTOR_STATE_PIDS_NAME "PIDs" -#define MOTOR_STATE_PIDS_NAME "PIDs" +#define FTS_NAME "FTs" -#define FTS_NAME "FTs" +const std::vector FTElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; -const std::vector FTElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; +#define GYROS_NAME "gyros" +const std::vector GyroElementNames = {"omega_x", "omega_y", "omega_z"}; -#define GYROS_NAME "gyros" -const std::vector GyroElementNames = {"omega_x", "omega_y", "omega_z"}; +#define ACCELEROMETERS_NAME "accelerometers" +const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; -#define ACCELEROMETERS_NAME "accelerometers" -const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; +#define ORIENTATIONS_NAME "orientations" +const std::vector OrientationElementNames = {"r", "p", "y"}; -#define ORIENTATIONS_NAME "orientations" -const std::vector OrientationElementNames = {"r", "p", "y"}; +#define MAGNETOMETERS_NAME "magnetometers" +const std::vector MagnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; -#define MAGNETOMETERS_NAME "magnetometers" -const std::vector MagnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; +#define CARTESIAN_WRENCHES_NAME "cartesian_wrenches" +const std::vector CartesianWrenchNames = {FTElementNames[0], + FTElementNames[1], + FTElementNames[2], + FTElementNames[3], + FTElementNames[4], + FTElementNames[5]}; -#define CARTESIAN_WRENCHES_NAME "cartesian_wrenches" -const std::vector CartesianWrenchNames = {FTElementNames[0], FTElementNames[1], FTElementNames[2], FTElementNames[3], FTElementNames[4], FTElementNames[5]}; +#define TEMPERATURE_NAME "temperatures" +const std::vector TemperatureNames = {"temperature"}; -#define TEMPERATURE_NAME "temperatures" -const std::vector TemperatureNames = {"temperature"}; +#define YARP_NAME "yarp_robot_name" -#define YARP_NAME "yarp_robot_name" +#define ROBOT_DESCRIPTON_LIST "description_list" -#define ROBOT_DESCRIPTON_LIST "description_list" - -#define TIMESTAMPS_NAME "timestamps" +#define TIMESTAMPS_NAME "timestamps" #endif - - - - diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpTextLoggingUtilities.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpTextLoggingUtilities.h index 7300f2e237..6fe0eae866 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpTextLoggingUtilities.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpTextLoggingUtilities.h @@ -6,8 +6,8 @@ #ifndef BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_YARP_TEXT_LOGGING_UTILITIES_H #define BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_YARP_TEXT_LOGGING_UTILITIES_H -#include #include +#include #include @@ -57,5 +57,4 @@ struct TextLoggingEntry } // namespace BipedalLocomotion - #endif // BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_YARP_TEXT_LOGGING_UTILITIES_H diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 6645001108..25ca3ec8b0 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -65,7 +65,9 @@ VISITABLE_STRUCT(TextLoggingEntry, yarprun_timestamp, local_timestamp); -void findAndReplaceAll(std::string& data, const std::string& toSearch, const std::string& replaceStr) +void findAndReplaceAll(std::string& data, + const std::string& toSearch, + const std::string& replaceStr) { // Get the first occurrence size_t pos = data.find(toSearch); @@ -103,7 +105,6 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, // the logging message are streamed using yarp BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); - } YarpRobotLoggerDevice::YarpRobotLoggerDevice() @@ -116,7 +117,6 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() // the logging message are streamed using yarp BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); - } YarpRobotLoggerDevice::~YarpRobotLoggerDevice() = default; @@ -612,7 +612,8 @@ bool YarpRobotLoggerDevice::setupRobotCameraBridge( return true; } -bool YarpRobotLoggerDevice::addChannelAndMetadata(const std::string& nameKey, const std::vector& metadataNames) +bool YarpRobotLoggerDevice::addChannelAndMetadata(const std::string& nameKey, + const std::vector& metadataNames) { bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames}); std::string rtNameKey = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + nameKey; @@ -620,7 +621,9 @@ bool YarpRobotLoggerDevice::addChannelAndMetadata(const std::string& nameKey, co return ok; } -void YarpRobotLoggerDevice::storeAndSendLoggingData(const std::string& name, const Eigen::VectorXd& data, const double time) +void YarpRobotLoggerDevice::storeAndSendLoggingData(const std::string& name, + const Eigen::VectorXd& data, + const double time) { m_bufferManager.push_back(data, time, name); std::string rtName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + name; @@ -669,19 +672,20 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } char* tmp = std::getenv("YARP_ROBOT_NAME"); - std::string metadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(YARP_NAME); + std::string metadataName + = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(YARP_NAME); m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); - metadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); + metadataName + = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); m_vectorCollectionRTDataServer.populateMetadata(metadataName, {TIMESTAMPS_NAME}); - const unsigned dofs = joints.size(); m_bufferManager.setDescriptionList(joints); - std::string rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(ROBOT_DESCRIPTON_LIST); + std::string rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + + std::string(ROBOT_DESCRIPTON_LIST); m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); - // prepare the telemetry if (m_streamJointStates) { @@ -712,7 +716,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { - std::string fullFTSensorName = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullFTSensorName + = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; ok &= addChannelAndMetadata(fullFTSensorName, FTElementNames); } } @@ -721,34 +726,41 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { - std::string fullGyroSensorName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullGyroSensorName + = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; ok &= addChannelAndMetadata(fullGyroSensorName, GyroElementNames); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { - std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullAccelerometerSensorName + = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; ok &= addChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { - std::string fullOrientationsSensorName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullOrientationsSensorName + = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; ok &= addChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { - std::string fullMagnetometersSensorName = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullMagnetometersSensorName + = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; ok &= addChannelAndMetadata(fullMagnetometersSensorName, MagnetometerElementNames); } // an IMU contains a gyro accelerometer and an orientation sensor for (const auto& sensorName : m_robotSensorBridge->getIMUsList()) { - std::string fullAccelerometerSensorName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; - std::string fullGyroSensorName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; - std::string fullOrientationsSensorName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullAccelerometerSensorName + = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullGyroSensorName + = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullOrientationsSensorName + = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; ok &= addChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); ok &= addChannelAndMetadata(fullGyroSensorName, GyroElementNames); ok &= addChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); @@ -759,7 +771,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { - std::string fullCartesianWrenchName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; + std::string fullCartesianWrenchName = std::string(CARTESIAN_WRENCHES_NAME) + + std::string(TREE_DELIM) + cartesianWrenchName; ok &= addChannelAndMetadata(fullCartesianWrenchName, CartesianWrenchNames); } } @@ -768,7 +781,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { - std::string fullTemperatureSensorName = std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; + std::string fullTemperatureSensorName + = std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; ok &= addChannelAndMetadata(fullTemperatureSensorName, TemperatureNames); } } @@ -777,7 +791,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); - BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection = signal.client.readData(false); + BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection + = signal.client.readData(false); if (externalSignalCollection != nullptr) { for (const auto& [key, vector] : externalSignalCollection->vectors) @@ -791,7 +806,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) logPrefix, signalName); m_bufferManager.addChannel({signalName, {vector.size(), 1}}); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; + rtSignalName + = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); } else @@ -800,8 +816,10 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) m_bufferManager.addChannel({signalName, // {vector.size(), 1}, metadata->second}); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; - m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {metadata->second}); + rtSignalName + = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; + m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, + {metadata->second}); } } } @@ -817,7 +835,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signal.signalName; + rtSignalName + = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signal.signalName; m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); signal.dataArrived = true; } @@ -829,7 +848,6 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // resize the temporary vectors m_jointSensorBuffer.resize(dofs); - // The user can avoid to record the camera if (m_cameraBridge != nullptr) { @@ -1073,8 +1091,9 @@ void YarpRobotLoggerDevice::lookForExogenousSignals() // if the connection is successful, get the metadata // this is required only for the vectors collection signal - if constexpr (std::is_same_v, - typename decltype(this->m_vectorsCollectionSignals)::mapped_type>) + if constexpr (std::is_same_v< + std::decay_t, + typename decltype(this->m_vectorsCollectionSignals)::mapped_type>) { if (!signal.connected) { @@ -1312,7 +1331,6 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit } } - void YarpRobotLoggerDevice::run() { constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; @@ -1334,7 +1352,8 @@ void YarpRobotLoggerDevice::run() std::string signalName = ""; std::string rtSignalName = ""; - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); + rtSignalName + = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); m_vectorCollectionRTDataServer.populateData(rtSignalName, timeData); if (m_streamJointStates) @@ -1420,7 +1439,7 @@ void YarpRobotLoggerDevice::run() } } - if(m_streamInertials) + if (m_streamInertials) { for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { @@ -1434,9 +1453,11 @@ void YarpRobotLoggerDevice::run() // pack the data for the accelerometer for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { - if(m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) + if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, + m_acceloremeterBuffer)) { - signalName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + signalName + = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; storeAndSendLoggingData(signalName, m_acceloremeterBuffer, time); } } @@ -1444,7 +1465,8 @@ void YarpRobotLoggerDevice::run() // pack the data for the orientations for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { - if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) + if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, + m_orientationBuffer)) { signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; storeAndSendLoggingData(signalName, m_orientationBuffer, time); @@ -1480,7 +1502,6 @@ void YarpRobotLoggerDevice::run() signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; storeAndSendLoggingData(signalName, m_orientationBuffer, time); - } } @@ -1490,7 +1511,8 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) { - signalName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + cartesianWrenchName; + signalName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + + cartesianWrenchName; storeAndSendLoggingData(signalName, m_ftBuffer, time); } } @@ -1500,20 +1522,21 @@ void YarpRobotLoggerDevice::run() { std::lock_guard lock(signal.mutex); - BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection = signal.client.readData(false); + BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection + = signal.client.readData(false); if (externalSignalCollection != nullptr) { for (const auto& [key, vector] : externalSignalCollection->vectors) { signalName = signal.signalName + std::string(TREE_DELIM) + key; m_bufferManager.push_back(vector, time, signalName); - rtSignalName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; + rtSignalName + = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; m_vectorCollectionRTDataServer.populateData(rtSignalName, vector); } } } - m_textLoggingPortMutex.lock(); int bufferportSize = m_textLoggingPort.getPendingReads(); BipedalLocomotion::TextLoggingEntry msg; @@ -1526,21 +1549,19 @@ void YarpRobotLoggerDevice::run() msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, std::to_string(time)); if (msg.isValid) { - signalName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName - + "::p" + msg.processPID; + signalName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName + "::p" + + msg.processPID; // matlab does not support the character - as a key of a struct findAndReplaceAll(signalName, "-", "_"); // if it is the first time this signal is seen by the device the channel is // added - if (m_textLogsStoredInManager.find(signalName) - == m_textLogsStoredInManager.end()) + if (m_textLogsStoredInManager.find(signalName) == m_textLogsStoredInManager.end()) { m_bufferManager.addChannel({signalName, {1, 1}}); m_textLogsStoredInManager.insert(signalName); } - } bufferportSize = m_textLoggingPort.getPendingReads(); } else @@ -1550,7 +1571,7 @@ void YarpRobotLoggerDevice::run() } m_textLoggingPortMutex.unlock(); - m_vectorCollectionRTDataServer.sendData(); + m_vectorCollectionRTDataServer.sendData(); } bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, From 2b6ef20ce609aede61e8f2f4972a5c0992f3693b Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 25 Jan 2024 09:53:11 +0100 Subject: [PATCH 45/83] Removed unused functions --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index bd1dbe53e7..dfc56be913 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -202,10 +202,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string& camera, const std::string& imageType); - // for realtime data streaming - void SendDataToLoggerVisualizer(); - - void ConfigureVectorCollectionServer(); }; } // namespace BipedalLocomotion From 2ccff2d516d43636d7f4cc0468f04eb4b75c57ea Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 25 Jan 2024 10:10:22 +0100 Subject: [PATCH 46/83] Fixed issue with header definition --- .../YarpRobotLoggerDeviceVariableTreeNames.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h index 5aa7e7faf6..8cb52acdd8 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h @@ -1,9 +1,9 @@ #ifndef BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H +#define BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H + #include #include -#define BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H - #define TREE_DELIM "::" #define ROBOT_RT_ROOT_NAME "robot_realtime" From 5ef8bb4ad31b8867414c376c7aeeba078acec343 Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 25 Jan 2024 10:10:45 +0100 Subject: [PATCH 47/83] Moved to lock guard instead of mutex lock --- .../src/YarpRobotLoggerDevice.cpp | 83 ++++++++++--------- 1 file changed, 44 insertions(+), 39 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index e1a6e1213a..278206f751 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1179,22 +1179,25 @@ void YarpRobotLoggerDevice::lookForNewLogs() // check for new messages yarp::profiler::NetworkProfiler::getPortsList(yarpPorts); - m_textLoggingPortMutex.lock(); - for (const auto& port : yarpPorts) - { - // check if the port has not be already connected if exits, its resposive - // it is a text logging port and it should be logged - if ((port.name.rfind(textLoggingPortPrefix, 0) == 0) - && (m_textLoggingPortNames.find(port.name) == m_textLoggingPortNames.end()) - && (m_textLoggingSubnames.empty() - || this->hasSubstring(port.name, m_textLoggingSubnames)) - && yarp::os::Network::exists(port.name)) + + // make a new scope for lock guarding text logging port + { + std::lock_guard lockGuardTextLogging(m_textLoggingPortMutex); + for (const auto& port : yarpPorts) { - m_textLoggingPortNames.insert(port.name); - yarp::os::Network::connect(port.name, m_textLoggingPortName, "udp"); + // check if the port has not be already connected if exits, its resposive + // it is a text logging port and it should be logged + if ((port.name.rfind(textLoggingPortPrefix, 0) == 0) + && (m_textLoggingPortNames.find(port.name) == m_textLoggingPortNames.end()) + && (m_textLoggingSubnames.empty() + || this->hasSubstring(port.name, m_textLoggingSubnames)) + && yarp::os::Network::exists(port.name)) + { + m_textLoggingPortNames.insert(port.name); + yarp::os::Network::connect(port.name, m_textLoggingPortName, "udp"); + } } - } - m_textLoggingPortMutex.unlock(); + } // end of scope for lock guarding text logging port // release the CPU BipedalLocomotion::clock().yield(); @@ -1538,39 +1541,41 @@ void YarpRobotLoggerDevice::run() } } - m_textLoggingPortMutex.lock(); - int bufferportSize = m_textLoggingPort.getPendingReads(); - BipedalLocomotion::TextLoggingEntry msg; - - while (bufferportSize > 0) + // lock guard scope for text logging port { - yarp::os::Bottle* b = m_textLoggingPort.read(false); - if (b != nullptr) + std::lock_guard lockGuardTextLogging(m_textLoggingPortMutex); + int bufferportSize = m_textLoggingPort.getPendingReads(); + BipedalLocomotion::TextLoggingEntry msg; + + while (bufferportSize > 0) { - msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, std::to_string(time)); - if (msg.isValid) + yarp::os::Bottle* b = m_textLoggingPort.read(false); + if (b != nullptr) { - signalName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName + "::p" - + msg.processPID; + msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, std::to_string(time)); + if (msg.isValid) + { + signalName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName + "::p" + + msg.processPID; - // matlab does not support the character - as a key of a struct - findAndReplaceAll(signalName, "-", "_"); + // matlab does not support the character - as a key of a struct + findAndReplaceAll(signalName, "-", "_"); - // if it is the first time this signal is seen by the device the channel is - // added - if (m_textLogsStoredInManager.find(signalName) == m_textLogsStoredInManager.end()) - { - m_bufferManager.addChannel({signalName, {1, 1}}); - m_textLogsStoredInManager.insert(signalName); + // if it is the first time this signal is seen by the device the channel is + // added + if (m_textLogsStoredInManager.find(signalName) == m_textLogsStoredInManager.end()) + { + m_bufferManager.addChannel({signalName, {1, 1}}); + m_textLogsStoredInManager.insert(signalName); + } } + bufferportSize = m_textLoggingPort.getPendingReads(); + } else + { + break; } - bufferportSize = m_textLoggingPort.getPendingReads(); - } else - { - break; } - } - m_textLoggingPortMutex.unlock(); + } // end of lock guard scope for text logging port m_vectorCollectionRTDataServer.sendData(); } From d24e88673587f4a82d62a7a43c2774ce05297ced Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 25 Jan 2024 10:15:01 +0100 Subject: [PATCH 48/83] Removed whitespace --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 278206f751..6c01a3f96e 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1179,7 +1179,7 @@ void YarpRobotLoggerDevice::lookForNewLogs() // check for new messages yarp::profiler::NetworkProfiler::getPortsList(yarpPorts); - + // make a new scope for lock guarding text logging port { std::lock_guard lockGuardTextLogging(m_textLoggingPortMutex); From 482e19574adfc84364d2e20ba286fed5b91d0295 Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 1 Feb 2024 17:53:38 +0100 Subject: [PATCH 49/83] Fixed data prepare bug with vector server --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 6c01a3f96e..14f597ca56 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1337,8 +1337,8 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit void YarpRobotLoggerDevice::run() { constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; - m_vectorCollectionRTDataServer.clearData(); m_vectorCollectionRTDataServer.prepareData(); + m_vectorCollectionRTDataServer.clearData(); // get the data if (!m_robotSensorBridge->advance()) From 841138c6da0650f1056f8335d994cb9b5f051261 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 5 Feb 2024 16:08:28 +0100 Subject: [PATCH 50/83] Cleaned up the code regarding constants --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 54 +++++++++++++- .../YarpRobotLoggerDeviceVariableTreeNames.h | 57 --------------- .../src/YarpRobotLoggerDevice.cpp | 73 +++++++++---------- 3 files changed, 89 insertions(+), 95 deletions(-) delete mode 100644 devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index dfc56be913..342f97a86a 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -178,7 +179,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool addChannelAndMetadata(const std::string& nameKey, const std::vector& metadata); void storeAndSendLoggingData(const std::string& name, - const Eigen::VectorXd& data, + const Eigen::Ref data, const double time); bool hasSubstring(const std::string& str, const std::vector& substrings) const; @@ -202,6 +203,57 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string& camera, const std::string& imageType); + const std::string TREE_DELIM = "::"; + + const std::string ROBOT_RT_ROOT_NAME = "robot_realtime"; + + const std::string JOINT_STATE_POSITIONS_NAME = "joints_state::positions"; + const std::string JOINT_STATE_VELOCITIES_NAME = "joints_state::velocities"; + const std::string JOINT_STATE_ACCLERATIONS_NAME = "joints_state::accelerations"; + const std::string JOINT_STATE_TORQUES_NAME = "joints_state::torques"; + + const std::string MOTOR_STATE_POSITIONS_NAME = "motors_state::positions"; + const std::string MOTOR_STATE_VELOCITIES_NAME = "motors_state::velocities"; + const std::string MOTOR_STATE_ACCELERATIONS_NAME = "motors_state::acclerations"; + const std::string MOTOR_STATE_CURRENTS_NAME = "motors_state::currents"; + const std::string MOTOR_STATE_PWM_NAME = "motors_state::PWM"; + + const std::string MOTOR_STATE_PIDS_NAME = "PIDs"; + + const std::string FTS_NAME = "FTs"; + + const std::vector FTElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; + + const std::string GYROS_NAME = "gyros"; + const std::vector GyroElementNames = {"omega_x", "omega_y", "omega_z"}; + + const std::string ACCELEROMETERS_NAME = "accelerometers"; + const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; + + const std::string ORIENTATIONS_NAME = "orientations"; + const std::vector OrientationElementNames = {"r", "p", "y"}; + + const std::string MAGNETOMETERS_NAME = "magnetometers"; + const std::vector MagnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; + + const std::string CARTESIAN_WRENCHES_NAME = "cartesian_wrenches"; + const std::vector CartesianWrenchNames = {FTElementNames[0], + FTElementNames[1], + FTElementNames[2], + FTElementNames[3], + FTElementNames[4], + FTElementNames[5]}; + + const std::string TEMPERATURE_NAME = "temperatures"; + const std::vector TemperatureNames = {"temperature"}; + + const std::string YARP_NAME = "yarp_robot_name"; + + const std::string ROBOT_DESCRIPTON_LIST = "description_list"; + + const std::string TIMESTAMPS_NAME = "timestamps"; + + }; } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h deleted file mode 100644 index 8cb52acdd8..0000000000 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDeviceVariableTreeNames.h +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H -#define BIPEDAL_LOCOMOTION_fRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_VTN_H - -#include -#include - -#define TREE_DELIM "::" - -#define ROBOT_RT_ROOT_NAME "robot_realtime" - -#define JOINT_STATE_POSITIONS_NAME "joints_state::positions" -#define JOINT_STATE_VELOCITIES_NAME "joints_state::velocities" -#define JOINT_STATE_ACCLERATIONS_NAME "joints_state::accelerations" -#define JOINT_STATE_TORQUES_NAME "joints_state::torques" - -#define MOTOR_STATE_POSITIONS_NAME "motors_state::positions" -#define MOTOR_STATE_VELOCITIES_NAME "motors_state::velocities" -#define MOTOR_STATE_ACCELERATIONS_NAME "motors_state::acclerations" -#define MOTOR_STATE_CURRENTS_NAME "motors_state::currents" -#define MOTOR_STATE_PWM_NAME "motors_state::PWM" - -#define MOTOR_STATE_PIDS_NAME "PIDs" - -#define FTS_NAME "FTs" - -const std::vector FTElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; - -#define GYROS_NAME "gyros" -const std::vector GyroElementNames = {"omega_x", "omega_y", "omega_z"}; - -#define ACCELEROMETERS_NAME "accelerometers" -const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; - -#define ORIENTATIONS_NAME "orientations" -const std::vector OrientationElementNames = {"r", "p", "y"}; - -#define MAGNETOMETERS_NAME "magnetometers" -const std::vector MagnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; - -#define CARTESIAN_WRENCHES_NAME "cartesian_wrenches" -const std::vector CartesianWrenchNames = {FTElementNames[0], - FTElementNames[1], - FTElementNames[2], - FTElementNames[3], - FTElementNames[4], - FTElementNames[5]}; - -#define TEMPERATURE_NAME "temperatures" -const std::vector TemperatureNames = {"temperature"}; - -#define YARP_NAME "yarp_robot_name" - -#define ROBOT_DESCRIPTON_LIST "description_list" - -#define TIMESTAMPS_NAME "timestamps" - -#endif diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 14f597ca56..e2b1043dcb 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -26,7 +26,6 @@ #include #include #include -#include #include #include #include @@ -616,17 +615,17 @@ bool YarpRobotLoggerDevice::addChannelAndMetadata(const std::string& nameKey, const std::vector& metadataNames) { bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames}); - std::string rtNameKey = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + nameKey; + std::string rtNameKey = ROBOT_RT_ROOT_NAME + TREE_DELIM + nameKey; ok &= m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames); return ok; } void YarpRobotLoggerDevice::storeAndSendLoggingData(const std::string& name, - const Eigen::VectorXd& data, + const Eigen::Ref data, const double time) { m_bufferManager.push_back(data, time, name); - std::string rtName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + name; + std::string rtName = ROBOT_RT_ROOT_NAME + TREE_DELIM + name; m_vectorCollectionRTDataServer.populateData(rtName, data); } @@ -673,17 +672,17 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) char* tmp = std::getenv("YARP_ROBOT_NAME"); std::string metadataName - = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(YARP_NAME); - m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); + = ROBOT_RT_ROOT_NAME + TREE_DELIM + YARP_NAME; + m_vectorCollectionRTDataServer.populateMetadata(metadataName, {tmp}); metadataName - = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); + = ROBOT_RT_ROOT_NAME + TREE_DELIM + TIMESTAMPS_NAME; m_vectorCollectionRTDataServer.populateMetadata(metadataName, {TIMESTAMPS_NAME}); const unsigned dofs = joints.size(); m_bufferManager.setDescriptionList(joints); - std::string rtMetadataName = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) - + std::string(ROBOT_DESCRIPTON_LIST); + std::string rtMetadataName = ROBOT_RT_ROOT_NAME + TREE_DELIM + + ROBOT_DESCRIPTON_LIST; m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); // prepare the telemetry @@ -717,7 +716,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { std::string fullFTSensorName - = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; + = FTS_NAME + TREE_DELIM + sensorName; ok &= addChannelAndMetadata(fullFTSensorName, FTElementNames); } } @@ -727,28 +726,28 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { std::string fullGyroSensorName - = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + = GYROS_NAME + TREE_DELIM + sensorName; ok &= addChannelAndMetadata(fullGyroSensorName, GyroElementNames); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { std::string fullAccelerometerSensorName - = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; ok &= addChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { std::string fullOrientationsSensorName - = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + = ORIENTATIONS_NAME + TREE_DELIM + sensorName; ok &= addChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { std::string fullMagnetometersSensorName - = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + = MAGNETOMETERS_NAME + TREE_DELIM + sensorName; ok &= addChannelAndMetadata(fullMagnetometersSensorName, MagnetometerElementNames); } @@ -756,11 +755,11 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getIMUsList()) { std::string fullAccelerometerSensorName - = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; std::string fullGyroSensorName - = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + = GYROS_NAME + TREE_DELIM + sensorName; std::string fullOrientationsSensorName - = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + = ORIENTATIONS_NAME + TREE_DELIM + sensorName; ok &= addChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); ok &= addChannelAndMetadata(fullGyroSensorName, GyroElementNames); ok &= addChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); @@ -771,8 +770,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { - std::string fullCartesianWrenchName = std::string(CARTESIAN_WRENCHES_NAME) - + std::string(TREE_DELIM) + cartesianWrenchName; + std::string fullCartesianWrenchName = CARTESIAN_WRENCHES_NAME + + TREE_DELIM + cartesianWrenchName; ok &= addChannelAndMetadata(fullCartesianWrenchName, CartesianWrenchNames); } } @@ -782,7 +781,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { std::string fullTemperatureSensorName - = std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; + = TEMPERATURE_NAME + TREE_DELIM + sensorName; ok &= addChannelAndMetadata(fullTemperatureSensorName, TemperatureNames); } } @@ -797,7 +796,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& [key, vector] : externalSignalCollection->vectors) { - signalName = signal.signalName + std::string(TREE_DELIM) + key; + signalName = signal.signalName + TREE_DELIM + key; const auto& metadata = signal.metadata.vectors.find(key); if (metadata == signal.metadata.vectors.cend()) { @@ -807,7 +806,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) signalName); m_bufferManager.addChannel({signalName, {vector.size(), 1}}); rtSignalName - = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; + = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); } else @@ -817,7 +816,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) {vector.size(), 1}, metadata->second}); rtSignalName - = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; + = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {metadata->second}); } @@ -836,7 +835,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); rtSignalName - = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signal.signalName; + = ROBOT_RT_ROOT_NAME + TREE_DELIM + signal.signalName; m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); signal.dataArrived = true; } @@ -1357,7 +1356,7 @@ void YarpRobotLoggerDevice::run() std::string rtSignalName = ""; rtSignalName - = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + std::string(TIMESTAMPS_NAME); + = ROBOT_RT_ROOT_NAME + TREE_DELIM + TIMESTAMPS_NAME; m_vectorCollectionRTDataServer.populateData(rtSignalName, timeData); if (m_streamJointStates) @@ -1422,7 +1421,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { - signalName = std::string(FTS_NAME) + std::string(TREE_DELIM) + sensorName; + signalName = FTS_NAME + TREE_DELIM + sensorName; storeAndSendLoggingData(signalName, m_ftBuffer, time); } } @@ -1434,7 +1433,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) { - signalName = std::string(TEMPERATURE_NAME) + std::string(TREE_DELIM) + sensorName; + signalName = TEMPERATURE_NAME + TREE_DELIM + sensorName; Eigen::Matrix temperatureData; temperatureData << m_ftTemperatureBuffer; @@ -1449,7 +1448,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { - signalName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + signalName = GYROS_NAME + TREE_DELIM + sensorName; storeAndSendLoggingData(signalName, m_gyroBuffer, time); } } @@ -1461,7 +1460,7 @@ void YarpRobotLoggerDevice::run() m_acceloremeterBuffer)) { signalName - = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; storeAndSendLoggingData(signalName, m_acceloremeterBuffer, time); } } @@ -1472,7 +1471,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) { - signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + signalName = ORIENTATIONS_NAME + TREE_DELIM + sensorName; storeAndSendLoggingData(signalName, m_orientationBuffer, time); } } @@ -1481,7 +1480,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { - signalName = std::string(MAGNETOMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + signalName = MAGNETOMETERS_NAME + TREE_DELIM + sensorName; storeAndSendLoggingData(signalName, m_magnemetometerBuffer, time); } } @@ -1498,13 +1497,13 @@ void YarpRobotLoggerDevice::run() m_gyroBuffer, m_orientationBuffer); - signalName = std::string(ACCELEROMETERS_NAME) + std::string(TREE_DELIM) + sensorName; + signalName = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; storeAndSendLoggingData(signalName, m_acceloremeterBuffer, time); - signalName = std::string(GYROS_NAME) + std::string(TREE_DELIM) + sensorName; + signalName = GYROS_NAME + TREE_DELIM + sensorName; storeAndSendLoggingData(signalName, m_gyroBuffer, time); - signalName = std::string(ORIENTATIONS_NAME) + std::string(TREE_DELIM) + sensorName; + signalName = ORIENTATIONS_NAME + TREE_DELIM + sensorName; storeAndSendLoggingData(signalName, m_orientationBuffer, time); } } @@ -1515,7 +1514,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) { - signalName = std::string(CARTESIAN_WRENCHES_NAME) + std::string(TREE_DELIM) + signalName = CARTESIAN_WRENCHES_NAME + TREE_DELIM + cartesianWrenchName; storeAndSendLoggingData(signalName, m_ftBuffer, time); } @@ -1532,10 +1531,10 @@ void YarpRobotLoggerDevice::run() { for (const auto& [key, vector] : externalSignalCollection->vectors) { - signalName = signal.signalName + std::string(TREE_DELIM) + key; + signalName = signal.signalName + TREE_DELIM + key; m_bufferManager.push_back(vector, time, signalName); rtSignalName - = std::string(ROBOT_RT_ROOT_NAME) + std::string(TREE_DELIM) + signalName; + = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; m_vectorCollectionRTDataServer.populateData(rtSignalName, vector); } } From e5d0cdbfc572456f2994713fbb64510656f35745 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 5 Feb 2024 18:23:28 +0100 Subject: [PATCH 51/83] Added pybind functions for metadata --- .../YarpUtilities/VectorsCollection.h | 1 + bindings/python/YarpUtilities/src/Module.cpp | 1 + .../YarpUtilities/src/VectorsCollection.cpp | 40 +++++++++++++++++-- 3 files changed, 38 insertions(+), 4 deletions(-) diff --git a/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h b/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h index 101508ef8b..e856c950da 100644 --- a/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h +++ b/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h @@ -19,6 +19,7 @@ namespace YarpUtilities void CreateVectorsCollectionServer(pybind11::module& module); void CreateVectorsCollectionClient(pybind11::module& module); +void CreateVectorsCollectionMetadata(pybind11::module& module); } // namespace YarpUtilities } // namespace bindings diff --git a/bindings/python/YarpUtilities/src/Module.cpp b/bindings/python/YarpUtilities/src/Module.cpp index 2187217a9d..05f310d6db 100644 --- a/bindings/python/YarpUtilities/src/Module.cpp +++ b/bindings/python/YarpUtilities/src/Module.cpp @@ -21,6 +21,7 @@ void CreateModule(pybind11::module& module) CreateVectorsCollectionServer(module); CreateVectorsCollectionClient(module); + CreateVectorsCollectionMetadata(module); } } // namespace IK } // namespace bindings diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index faee792d69..3dd1a101e3 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -64,20 +64,52 @@ void CreateVectorsCollectionClient(pybind11::module& module) py::arg("handler")) .def("connect", &VectorsCollectionClient::connect) .def("disconnect", &VectorsCollectionClient::disconnect) - .def("getMetadata", - [](VectorsCollectionClient& impl) -> std::map> + .def("get_metadata", + [](VectorsCollectionClient& impl) -> BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata { BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata metadata; impl.getMetadata(metadata); - return metadata.vectors; + return metadata; }) - .def("readData", + .def("read_data", [](VectorsCollectionClient& impl, bool shouldWait) -> std::map> { BipedalLocomotion::YarpUtilities::VectorsCollection* collection = impl.readData(shouldWait); return collection->vectors; }); } + +void CreateVectorsCollectionMetadata(pybind11::module& module) +{ + namespace py = ::pybind11; + + using namespace ::BipedalLocomotion::YarpUtilities; + + py::class_(module, "VectorsCollectionMetadata") + .def(py::init()) + .def(py::init>&>()) + .def("readWireReader", + [](VectorsCollectionMetadata& impl, yarp::os::idl::WireReader& reader) -> bool { + return impl.read(reader); + }) + .def("readConnectionReader", + [](VectorsCollectionMetadata& impl, yarp::os::ConnectionReader& connection) -> bool { + return impl.read(connection); + }) + .def("writeWireWriter", + [](const VectorsCollectionMetadata& impl, const yarp::os::idl::WireWriter& writer) -> bool { + return impl.write(writer); + }) + .def("writeConnectionWriter", + [](const VectorsCollectionMetadata& impl, yarp::os::ConnectionWriter& connection) -> bool { + return impl.write(connection); + }) + .def("toString", &VectorsCollectionMetadata::toString) + .def("getVectors", + [](const VectorsCollectionMetadata& impl) -> std::map> { + return impl.vectors; + }); +} } // namespace YarpUtilities } // namespace bindings } // namespace BipedalLocomotion From 568addd28da23a7150b502c63ccfb31cb508e6b6 Mon Sep 17 00:00:00 2001 From: Nick Date: Mon, 5 Feb 2024 18:23:59 +0100 Subject: [PATCH 52/83] cleaned up variable locations --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 5 ++--- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 342f97a86a..1890b8f992 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -179,7 +180,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool addChannelAndMetadata(const std::string& nameKey, const std::vector& metadata); void storeAndSendLoggingData(const std::string& name, - const Eigen::Ref data, + const Eigen::VectorXd& data, const double time); bool hasSubstring(const std::string& str, const std::vector& substrings) const; @@ -252,8 +253,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string ROBOT_DESCRIPTON_LIST = "description_list"; const std::string TIMESTAMPS_NAME = "timestamps"; - - }; } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index e2b1043dcb..747f3ffdc5 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -621,7 +621,7 @@ bool YarpRobotLoggerDevice::addChannelAndMetadata(const std::string& nameKey, } void YarpRobotLoggerDevice::storeAndSendLoggingData(const std::string& name, - const Eigen::Ref data, + const Eigen::VectorXd& data, const double time) { m_bufferManager.push_back(data, time, name); @@ -673,7 +673,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) char* tmp = std::getenv("YARP_ROBOT_NAME"); std::string metadataName = ROBOT_RT_ROOT_NAME + TREE_DELIM + YARP_NAME; - m_vectorCollectionRTDataServer.populateMetadata(metadataName, {tmp}); + m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); metadataName = ROBOT_RT_ROOT_NAME + TREE_DELIM + TIMESTAMPS_NAME; From 52499b55a93bf6811157a3f5118ff3952010b968 Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 8 Feb 2024 11:28:35 +0100 Subject: [PATCH 53/83] Added an option to open the RT port from XML --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 9 + .../src/YarpRobotLoggerDevice.cpp | 221 +++++++++++------- 2 files changed, 143 insertions(+), 87 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 1890b8f992..c385e1a175 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -65,6 +65,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::unique_ptr m_robotSensorBridge; std::unique_ptr m_cameraBridge; + bool sendDataRT; BipedalLocomotion::YarpUtilities::VectorsCollectionServer m_vectorCollectionRTDataServer; template struct ExogenousSignal @@ -182,6 +183,14 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void storeAndSendLoggingData(const std::string& name, const Eigen::VectorXd& data, const double time); + bool addChannel(const std::string& nameKey, const std::vector& metadata); + void storeLoggingData(const std::string& name, + const Eigen::VectorXd& data, + const double time); + bool (BipedalLocomotion::YarpRobotLoggerDevice::*initMetadataFunction)(const std::string& nameKey, const std::vector& metadata); + void (BipedalLocomotion::YarpRobotLoggerDevice::*loggingDataFunction)(const std::string& name, + const Eigen::VectorXd& data, + const double time); bool hasSubstring(const std::string& str, const std::vector& substrings) const; void recordVideo(const std::string& cameraName, VideoWriter& writer); diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 747f3ffdc5..0c0d3fefed 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -104,6 +104,8 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, // the logging message are streamed using yarp BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); + + sendDataRT = false; } YarpRobotLoggerDevice::YarpRobotLoggerDevice() @@ -116,6 +118,8 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() // the logging message are streamed using yarp BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); + + sendDataRT = false; } YarpRobotLoggerDevice::~YarpRobotLoggerDevice() = default; @@ -125,11 +129,26 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) constexpr auto logPrefix = "[YarpRobotLoggerDevice::open]"; auto params = std::make_shared(config); - if (!m_vectorCollectionRTDataServer.initialize(params)) + std::string remote; + sendDataRT = params->getParameter("remote", remote); + if (sendDataRT) { - log()->error("Failed to initalize the vectorsCollectionServer", logPrefix); - return false; + yInfo() << "Activating Real Time Logging on yarp port: " << remote; + initMetadataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::addChannelAndMetadata; + loggingDataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::storeAndSendLoggingData; + if (!m_vectorCollectionRTDataServer.initialize(params)) + { + log()->error("Failed to initalize the vectorsCollectionServer", logPrefix); + return false; + } + } + else + { + yInfo() << "Real time logging not activated"; + initMetadataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::addChannel; + loggingDataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::storeLoggingData; } + double devicePeriod{0.01}; if (params->getParameter("sampling_period_in_s", devicePeriod)) { @@ -629,6 +648,20 @@ void YarpRobotLoggerDevice::storeAndSendLoggingData(const std::string& name, m_vectorCollectionRTDataServer.populateData(rtName, data); } +bool YarpRobotLoggerDevice::addChannel(const std::string& nameKey, + const std::vector& metadataNames) +{ + bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames}); + return ok; +} + +void YarpRobotLoggerDevice::storeLoggingData(const std::string& name, + const Eigen::VectorXd& data, + const double time) +{ + m_bufferManager.push_back(data, time, name); +} + bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { constexpr auto logPrefix = "[YarpRobotLoggerDevice::attachAll]"; @@ -670,45 +703,48 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) return false; } - char* tmp = std::getenv("YARP_ROBOT_NAME"); - std::string metadataName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + YARP_NAME; - m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); - - metadataName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + TIMESTAMPS_NAME; - m_vectorCollectionRTDataServer.populateMetadata(metadataName, {TIMESTAMPS_NAME}); - const unsigned dofs = joints.size(); m_bufferManager.setDescriptionList(joints); - std::string rtMetadataName = ROBOT_RT_ROOT_NAME + TREE_DELIM - + ROBOT_DESCRIPTON_LIST; - m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + if(sendDataRT) + { + char* tmp = std::getenv("YARP_ROBOT_NAME"); + std::string metadataName + = ROBOT_RT_ROOT_NAME + TREE_DELIM + YARP_NAME; + m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); + + metadataName + = ROBOT_RT_ROOT_NAME + TREE_DELIM + TIMESTAMPS_NAME; + m_vectorCollectionRTDataServer.populateMetadata(metadataName, {TIMESTAMPS_NAME}); + + std::string rtMetadataName = ROBOT_RT_ROOT_NAME + TREE_DELIM + + ROBOT_DESCRIPTON_LIST; + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + } // prepare the telemetry if (m_streamJointStates) { - ok &= addChannelAndMetadata(JOINT_STATE_POSITIONS_NAME, joints); - ok &= addChannelAndMetadata(JOINT_STATE_VELOCITIES_NAME, joints); - ok &= addChannelAndMetadata(JOINT_STATE_ACCLERATIONS_NAME, joints); - ok &= addChannelAndMetadata(JOINT_STATE_TORQUES_NAME, joints); + ok &= (*this.*initMetadataFunction)(JOINT_STATE_POSITIONS_NAME, joints); + ok &= (*this.*initMetadataFunction)(JOINT_STATE_VELOCITIES_NAME, joints); + ok &= (*this.*initMetadataFunction)(JOINT_STATE_ACCLERATIONS_NAME, joints); + ok &= (*this.*initMetadataFunction)(JOINT_STATE_TORQUES_NAME, joints); } if (m_streamMotorStates) { - ok &= addChannelAndMetadata(MOTOR_STATE_POSITIONS_NAME, joints); - ok &= addChannelAndMetadata(MOTOR_STATE_VELOCITIES_NAME, joints); - ok &= addChannelAndMetadata(MOTOR_STATE_ACCELERATIONS_NAME, joints); - ok &= addChannelAndMetadata(MOTOR_STATE_CURRENTS_NAME, joints); + ok &= (*this.*initMetadataFunction)(MOTOR_STATE_POSITIONS_NAME, joints); + ok &= (*this.*initMetadataFunction)(MOTOR_STATE_VELOCITIES_NAME, joints); + ok &= (*this.*initMetadataFunction)(MOTOR_STATE_ACCELERATIONS_NAME, joints); + ok &= (*this.*initMetadataFunction)(MOTOR_STATE_CURRENTS_NAME, joints); } if (m_streamMotorPWM) { - ok &= addChannelAndMetadata(MOTOR_STATE_PWM_NAME, joints); + ok &= (*this.*initMetadataFunction)(MOTOR_STATE_PWM_NAME, joints); } if (m_streamPIDs) { - ok &= addChannelAndMetadata(MOTOR_STATE_PIDS_NAME, joints); + ok &= (*this.*initMetadataFunction)(MOTOR_STATE_PIDS_NAME, joints); } if (m_streamFTSensors) @@ -717,7 +753,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullFTSensorName = FTS_NAME + TREE_DELIM + sensorName; - ok &= addChannelAndMetadata(fullFTSensorName, FTElementNames); + ok &= (*this.*initMetadataFunction)(fullFTSensorName, FTElementNames); } } @@ -727,28 +763,28 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullGyroSensorName = GYROS_NAME + TREE_DELIM + sensorName; - ok &= addChannelAndMetadata(fullGyroSensorName, GyroElementNames); + ok &= (*this.*initMetadataFunction)(fullGyroSensorName, GyroElementNames); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { std::string fullAccelerometerSensorName = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; - ok &= addChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); + ok &= (*this.*initMetadataFunction)(fullAccelerometerSensorName, AccelerometerElementNames); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { std::string fullOrientationsSensorName = ORIENTATIONS_NAME + TREE_DELIM + sensorName; - ok &= addChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); + ok &= (*this.*initMetadataFunction)(fullOrientationsSensorName, OrientationElementNames); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { std::string fullMagnetometersSensorName = MAGNETOMETERS_NAME + TREE_DELIM + sensorName; - ok &= addChannelAndMetadata(fullMagnetometersSensorName, MagnetometerElementNames); + ok &= (*this.*initMetadataFunction)(fullMagnetometersSensorName, MagnetometerElementNames); } // an IMU contains a gyro accelerometer and an orientation sensor @@ -760,9 +796,9 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) = GYROS_NAME + TREE_DELIM + sensorName; std::string fullOrientationsSensorName = ORIENTATIONS_NAME + TREE_DELIM + sensorName; - ok &= addChannelAndMetadata(fullAccelerometerSensorName, AccelerometerElementNames); - ok &= addChannelAndMetadata(fullGyroSensorName, GyroElementNames); - ok &= addChannelAndMetadata(fullOrientationsSensorName, OrientationElementNames); + ok &= (*this.*initMetadataFunction)(fullAccelerometerSensorName, AccelerometerElementNames); + ok &= (*this.*initMetadataFunction)(fullGyroSensorName, GyroElementNames); + ok &= (*this.*initMetadataFunction)(fullOrientationsSensorName, OrientationElementNames); } } @@ -772,7 +808,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullCartesianWrenchName = CARTESIAN_WRENCHES_NAME + TREE_DELIM + cartesianWrenchName; - ok &= addChannelAndMetadata(fullCartesianWrenchName, CartesianWrenchNames); + ok &= (*this.*initMetadataFunction)(fullCartesianWrenchName, CartesianWrenchNames); } } @@ -782,7 +818,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullTemperatureSensorName = TEMPERATURE_NAME + TREE_DELIM + sensorName; - ok &= addChannelAndMetadata(fullTemperatureSensorName, TemperatureNames); + ok &= (*this.*initMetadataFunction)(fullTemperatureSensorName, TemperatureNames); } } std::string signalName = ""; @@ -805,20 +841,24 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) logPrefix, signalName); m_bufferManager.addChannel({signalName, {vector.size(), 1}}); - rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; - - m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); + if(sendDataRT) + { + rtSignalName + = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; + m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); + } } else { // if the metadata is found we use it m_bufferManager.addChannel({signalName, // {vector.size(), 1}, metadata->second}); - rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; - m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, - {metadata->second}); + if(sendDataRT) + { + rtSignalName + = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; + m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {metadata->second}); + } } } } @@ -834,15 +874,19 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); - rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + signal.signalName; - m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); + if(sendDataRT) + { + rtSignalName + = ROBOT_RT_ROOT_NAME + TREE_DELIM + signal.signalName; + m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); + } signal.dataArrived = true; } } } - m_vectorCollectionRTDataServer.finalizeMetadata(); + if(sendDataRT) + m_vectorCollectionRTDataServer.finalizeMetadata(); // resize the temporary vectors m_jointSensorBuffer.resize(dofs); @@ -1336,8 +1380,21 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit void YarpRobotLoggerDevice::run() { constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; - m_vectorCollectionRTDataServer.prepareData(); - m_vectorCollectionRTDataServer.clearData(); + const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); + std::string signalName = ""; + std::string rtSignalName = ""; + + std::lock_guard lock(m_bufferManagerMutex); + if(sendDataRT) + { + m_vectorCollectionRTDataServer.prepareData(); + m_vectorCollectionRTDataServer.clearData(); + Eigen::Matrix timeData; + timeData << time; + rtSignalName + = ROBOT_RT_ROOT_NAME + TREE_DELIM + TIMESTAMPS_NAME; + m_vectorCollectionRTDataServer.populateData(rtSignalName, timeData); + } // get the data if (!m_robotSensorBridge->advance()) @@ -1345,37 +1402,23 @@ void YarpRobotLoggerDevice::run() log()->error("{} Could not advance sensor bridge.", logPrefix); } - const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); - - std::lock_guard lock(m_bufferManagerMutex); - - Eigen::Matrix timeData; - timeData << time; - - std::string signalName = ""; - std::string rtSignalName = ""; - - rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + TIMESTAMPS_NAME; - m_vectorCollectionRTDataServer.populateData(rtSignalName, timeData); - if (m_streamJointStates) { if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) { - storeAndSendLoggingData(JOINT_STATE_POSITIONS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(JOINT_STATE_POSITIONS_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) { - storeAndSendLoggingData(JOINT_STATE_VELOCITIES_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(JOINT_STATE_VELOCITIES_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) { - storeAndSendLoggingData(JOINT_STATE_ACCLERATIONS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(JOINT_STATE_ACCLERATIONS_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { - storeAndSendLoggingData(JOINT_STATE_TORQUES_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(JOINT_STATE_TORQUES_NAME, m_jointSensorBuffer, time); } } @@ -1383,19 +1426,19 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { - storeAndSendLoggingData(MOTOR_STATE_POSITIONS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(MOTOR_STATE_POSITIONS_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { - storeAndSendLoggingData(MOTOR_STATE_VELOCITIES_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(MOTOR_STATE_VELOCITIES_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { - storeAndSendLoggingData(MOTOR_STATE_ACCELERATIONS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(MOTOR_STATE_ACCELERATIONS_NAME, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { - storeAndSendLoggingData(MOTOR_STATE_CURRENTS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(MOTOR_STATE_CURRENTS_NAME, m_jointSensorBuffer, time); } } @@ -1403,7 +1446,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) { - storeAndSendLoggingData(MOTOR_STATE_PWM_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(MOTOR_STATE_PWM_NAME, m_jointSensorBuffer, time); } } @@ -1411,7 +1454,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) { - storeAndSendLoggingData(MOTOR_STATE_PIDS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(MOTOR_STATE_PIDS_NAME, m_jointSensorBuffer, time); } } @@ -1422,7 +1465,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { signalName = FTS_NAME + TREE_DELIM + sensorName; - storeAndSendLoggingData(signalName, m_ftBuffer, time); + (*this.*loggingDataFunction)(signalName, m_ftBuffer, time); } } } @@ -1437,7 +1480,7 @@ void YarpRobotLoggerDevice::run() Eigen::Matrix temperatureData; temperatureData << m_ftTemperatureBuffer; - storeAndSendLoggingData(signalName, temperatureData, time); + (*this.*loggingDataFunction)(signalName, temperatureData, time); } } } @@ -1449,7 +1492,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { signalName = GYROS_NAME + TREE_DELIM + sensorName; - storeAndSendLoggingData(signalName, m_gyroBuffer, time); + (*this.*loggingDataFunction)(signalName, m_gyroBuffer, time); } } @@ -1461,7 +1504,7 @@ void YarpRobotLoggerDevice::run() { signalName = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; - storeAndSendLoggingData(signalName, m_acceloremeterBuffer, time); + (*this.*loggingDataFunction)(signalName, m_acceloremeterBuffer, time); } } @@ -1472,7 +1515,7 @@ void YarpRobotLoggerDevice::run() m_orientationBuffer)) { signalName = ORIENTATIONS_NAME + TREE_DELIM + sensorName; - storeAndSendLoggingData(signalName, m_orientationBuffer, time); + (*this.*loggingDataFunction)(signalName, m_orientationBuffer, time); } } @@ -1481,7 +1524,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { signalName = MAGNETOMETERS_NAME + TREE_DELIM + sensorName; - storeAndSendLoggingData(signalName, m_magnemetometerBuffer, time); + (*this.*loggingDataFunction)(signalName, m_magnemetometerBuffer, time); } } } @@ -1498,13 +1541,13 @@ void YarpRobotLoggerDevice::run() m_orientationBuffer); signalName = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; - storeAndSendLoggingData(signalName, m_acceloremeterBuffer, time); + (*this.*loggingDataFunction)(signalName, m_acceloremeterBuffer, time); signalName = GYROS_NAME + TREE_DELIM + sensorName; - storeAndSendLoggingData(signalName, m_gyroBuffer, time); + (*this.*loggingDataFunction)(signalName, m_gyroBuffer, time); signalName = ORIENTATIONS_NAME + TREE_DELIM + sensorName; - storeAndSendLoggingData(signalName, m_orientationBuffer, time); + (*this.*loggingDataFunction)(signalName, m_orientationBuffer, time); } } @@ -1516,7 +1559,7 @@ void YarpRobotLoggerDevice::run() { signalName = CARTESIAN_WRENCHES_NAME + TREE_DELIM + cartesianWrenchName; - storeAndSendLoggingData(signalName, m_ftBuffer, time); + (*this.*loggingDataFunction)(signalName, m_ftBuffer, time); } } } @@ -1533,9 +1576,12 @@ void YarpRobotLoggerDevice::run() { signalName = signal.signalName + TREE_DELIM + key; m_bufferManager.push_back(vector, time, signalName); - rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, vector); + if(sendDataRT) + { + rtSignalName + = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; + m_vectorCollectionRTDataServer.populateData(rtSignalName, vector); + } } } } @@ -1576,7 +1622,8 @@ void YarpRobotLoggerDevice::run() } } // end of lock guard scope for text logging port - m_vectorCollectionRTDataServer.sendData(); + if(sendDataRT) + m_vectorCollectionRTDataServer.sendData(); } bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, From 7244bca204f242ff4508b45cf6f898cae191aec5 Mon Sep 17 00:00:00 2001 From: Nick Date: Thu, 8 Feb 2024 11:59:23 +0100 Subject: [PATCH 54/83] Cleaned up the code a bit more with the function pointers --- .../src/YarpRobotLoggerDevice.cpp | 28 ++----------------- 1 file changed, 3 insertions(+), 25 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 0c0d3fefed..3cd3a06bab 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -840,25 +840,10 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) "default one will be used.", logPrefix, signalName); - m_bufferManager.addChannel({signalName, {vector.size(), 1}}); - if(sendDataRT) - { - rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; - m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); - } + (*this.*initMetadataFunction)(signalName, {}); } else { - // if the metadata is found we use it - m_bufferManager.addChannel({signalName, // - {vector.size(), 1}, - metadata->second}); - if(sendDataRT) - { - rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; - m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {metadata->second}); - } + (*this.*initMetadataFunction)(signalName, {metadata->second}); } } } @@ -872,14 +857,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { if (!signal.dataArrived) { - m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); - - if(sendDataRT) - { - rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + signal.signalName; - m_vectorCollectionRTDataServer.populateMetadata(rtSignalName, {}); - } + (*this.*initMetadataFunction)(signalName, {}); signal.dataArrived = true; } } From 51f9a8a22f10fcf3cb38a8dacfdc7c7e0f045182 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 13 Feb 2024 16:19:28 +0100 Subject: [PATCH 55/83] Fixed issue with vector collection server --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 1 + .../src/YarpRobotLoggerDevice.cpp | 104 ++++++++++++++---- .../src/VectorsCollectionServer.cpp | 16 +-- 3 files changed, 91 insertions(+), 30 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index c385e1a175..c1d65a0aaf 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -108,6 +108,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::unordered_map m_vectorsCollectionSignals; std::unordered_map> m_vectorSignals; + unsigned int m_updatedMetadataSignalVal; std::unordered_set m_exogenousPortsStoredInManager; std::atomic m_lookForNewExogenousSignalIsRunning{false}; diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 3cd3a06bab..ef92470d79 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -105,6 +105,7 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); + m_updatedMetadataSignalVal = 0; sendDataRT = false; } @@ -119,6 +120,7 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); + m_updatedMetadataSignalVal = 0; sendDataRT = false; } @@ -821,8 +823,10 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) ok &= (*this.*initMetadataFunction)(fullTemperatureSensorName, TemperatureNames); } } + std::string signalName = ""; std::string rtSignalName = ""; + for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); @@ -830,21 +834,26 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) = signal.client.readData(false); if (externalSignalCollection != nullptr) { - for (const auto& [key, vector] : externalSignalCollection->vectors) + if (!signal.dataArrived) { - signalName = signal.signalName + TREE_DELIM + 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, - signalName); - (*this.*initMetadataFunction)(signalName, {}); - } else + for (const auto& [key, vector] : externalSignalCollection->vectors) { - (*this.*initMetadataFunction)(signalName, {metadata->second}); + signalName = signal.signalName + TREE_DELIM + 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, + signalName); + (*this.*initMetadataFunction)(signalName, {}); + } else + { + std::cout << "Adding more metadata!!!" << std::endl; + (*this.*initMetadataFunction)(signalName, {metadata->second}); + } } + signal.dataArrived = true; } } } @@ -852,8 +861,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (auto& [name, signal] : m_vectorSignals) { std::lock_guard lock(signal.mutex); - yarp::sig::Vector* vector = signal.port.read(false); - if (vector != nullptr) + yarp::sig::Vector* collection = signal.port.read(false); + if (collection != nullptr) { if (!signal.dataArrived) { @@ -863,6 +872,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } } + m_vectorCollectionRTDataServer.populateMetadata(ROBOT_RT_ROOT_NAME + TREE_DELIM + "newMetadata", {"newMetadata"}); + if(sendDataRT) m_vectorCollectionRTDataServer.finalizeMetadata(); @@ -1541,7 +1552,8 @@ void YarpRobotLoggerDevice::run() } } } - + + bool newMetadata = false; for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); @@ -1550,16 +1562,64 @@ void YarpRobotLoggerDevice::run() = signal.client.readData(false); if (externalSignalCollection != nullptr) { - for (const auto& [key, vector] : externalSignalCollection->vectors) + if(!signal.dataArrived) + { + for (const auto& [key, vector] : externalSignalCollection->vectors) + { + signalName = signal.signalName + TREE_DELIM + 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, + signalName); + (*this.*initMetadataFunction)(signalName, {}); + } else + { + (*this.*initMetadataFunction)(signalName, {metadata->second}); + } + } + signal.dataArrived = true; + newMetadata = true; + } + else { - signalName = signal.signalName + TREE_DELIM + key; - m_bufferManager.push_back(vector, time, signalName); - if(sendDataRT) + for (const auto& [key, vector] : externalSignalCollection->vectors) { - rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, vector); + signalName = signal.signalName + TREE_DELIM + key; + m_bufferManager.push_back(vector, time, signalName); + if(sendDataRT) + { + rtSignalName + = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; + m_vectorCollectionRTDataServer.populateData(rtSignalName, vector); + } } + + } + } + } + Eigen::Matrix newMetadataVector; + if(newMetadata) + { + std::cout << "Adding new metadata!!" << std::endl; + m_updatedMetadataSignalVal = (m_updatedMetadataSignalVal + 1) % 2; + } + newMetadataVector << m_updatedMetadataSignalVal; + + m_vectorCollectionRTDataServer.populateData(ROBOT_RT_ROOT_NAME + TREE_DELIM + "newMetadata", newMetadataVector); + + for (auto& [name, signal] : m_vectorSignals) + { + std::lock_guard lock(signal.mutex); + yarp::sig::Vector* collection = signal.port.read(false); + if (collection != nullptr) + { + if (!signal.dataArrived) + { + (*this.*initMetadataFunction)(signalName, {}); + signal.dataArrived = true; } } } diff --git a/src/YarpUtilities/src/VectorsCollectionServer.cpp b/src/YarpUtilities/src/VectorsCollectionServer.cpp index 0284760bd2..f51e2a6a28 100644 --- a/src/YarpUtilities/src/VectorsCollectionServer.cpp +++ b/src/YarpUtilities/src/VectorsCollectionServer.cpp @@ -96,14 +96,14 @@ bool VectorsCollectionServer::populateMetadata(const std::string& key, const std::vector& metadata) { // check if the metadata has been already populated - if (m_pimpl->isMetadataFinalized) - { - log()->error("[VectorsCollectionServer::populateMetadata] The metadata has been already " - "populated."); - return false; - } - - // check if the key has been already used + // if (m_pimpl->isMetadataFinalized) + // { + // log()->error("[VectorsCollectionServer::populateMetadata] The metadata has been already " + // "populated."); + // return false; + // } + + // // check if the key has been already used if (m_pimpl->setOfKeys.find(key) != m_pimpl->setOfKeys.end()) { log()->error("[VectorsCollectionServer::populateMetadata] The key {} has been already " From cc97f403490bba0e570e6eec7c1f2c2f19af5ad5 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 13 Feb 2024 16:25:07 +0100 Subject: [PATCH 56/83] Cleaned up the code --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 3 --- src/YarpUtilities/src/VectorsCollectionServer.cpp | 3 +++ 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index ef92470d79..660887702c 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1602,10 +1602,7 @@ void YarpRobotLoggerDevice::run() } Eigen::Matrix newMetadataVector; if(newMetadata) - { - std::cout << "Adding new metadata!!" << std::endl; m_updatedMetadataSignalVal = (m_updatedMetadataSignalVal + 1) % 2; - } newMetadataVector << m_updatedMetadataSignalVal; m_vectorCollectionRTDataServer.populateData(ROBOT_RT_ROOT_NAME + TREE_DELIM + "newMetadata", newMetadataVector); diff --git a/src/YarpUtilities/src/VectorsCollectionServer.cpp b/src/YarpUtilities/src/VectorsCollectionServer.cpp index f51e2a6a28..e748589bc7 100644 --- a/src/YarpUtilities/src/VectorsCollectionServer.cpp +++ b/src/YarpUtilities/src/VectorsCollectionServer.cpp @@ -96,6 +96,9 @@ bool VectorsCollectionServer::populateMetadata(const std::string& key, const std::vector& metadata) { // check if the metadata has been already populated + // This code was temporarily removed via commenting it out, this will be discussed with the team + // later on how to append metadata after it is finalized + // if (m_pimpl->isMetadataFinalized) // { // log()->error("[VectorsCollectionServer::populateMetadata] The metadata has been already " From 609e3c24b12eb05c21c6ee0e428cada3ce39e930 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 13 Feb 2024 16:33:10 +0100 Subject: [PATCH 57/83] Removed whitespace --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 660887702c..00412d03ed 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1552,7 +1552,7 @@ void YarpRobotLoggerDevice::run() } } } - + bool newMetadata = false; for (auto& [name, signal] : m_vectorsCollectionSignals) { @@ -1604,7 +1604,7 @@ void YarpRobotLoggerDevice::run() if(newMetadata) m_updatedMetadataSignalVal = (m_updatedMetadataSignalVal + 1) % 2; newMetadataVector << m_updatedMetadataSignalVal; - + m_vectorCollectionRTDataServer.populateData(ROBOT_RT_ROOT_NAME + TREE_DELIM + "newMetadata", newMetadataVector); for (auto& [name, signal] : m_vectorSignals) From 3cfaf3db364de1bc9712d06299f2f560053d3d31 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 13 Feb 2024 16:33:59 +0100 Subject: [PATCH 58/83] Removed whitespace --- src/YarpUtilities/src/VectorsCollectionServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/YarpUtilities/src/VectorsCollectionServer.cpp b/src/YarpUtilities/src/VectorsCollectionServer.cpp index e748589bc7..1b84123318 100644 --- a/src/YarpUtilities/src/VectorsCollectionServer.cpp +++ b/src/YarpUtilities/src/VectorsCollectionServer.cpp @@ -98,7 +98,7 @@ bool VectorsCollectionServer::populateMetadata(const std::string& key, // check if the metadata has been already populated // This code was temporarily removed via commenting it out, this will be discussed with the team // later on how to append metadata after it is finalized - + // if (m_pimpl->isMetadataFinalized) // { // log()->error("[VectorsCollectionServer::populateMetadata] The metadata has been already " From 6c1fdd1752296e37d0096d253f4a703d27920478 Mon Sep 17 00:00:00 2001 From: Nick Date: Fri, 16 Feb 2024 11:55:11 +0100 Subject: [PATCH 59/83] Fixed but regarding closing and opening logger devices --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 3 +++ .../src/YarpRobotLoggerDevice.cpp | 13 ++++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index c1d65a0aaf..4ad5157507 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -99,6 +99,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, BipedalLocomotion::YarpUtilities::VectorsCollectionClient client; BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata metadata; std::string signalName; + unsigned int numMissedPackets; bool dataArrived{false}; bool connected{false}; @@ -214,6 +215,8 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string& camera, const std::string& imageType); + const unsigned int maxTimoutForExogenousSignal = 10; + const std::string TREE_DELIM = "::"; const std::string ROBOT_RT_ROOT_NAME = "robot_realtime"; diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 00412d03ed..56027a9a05 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -849,7 +849,6 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) (*this.*initMetadataFunction)(signalName, {}); } else { - std::cout << "Adding more metadata!!!" << std::endl; (*this.*initMetadataFunction)(signalName, {metadata->second}); } } @@ -1560,6 +1559,7 @@ void YarpRobotLoggerDevice::run() BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection = signal.client.readData(false); + if (externalSignalCollection != nullptr) { if(!signal.dataArrived) @@ -1581,10 +1581,12 @@ void YarpRobotLoggerDevice::run() } } signal.dataArrived = true; + signal.numMissedPackets = 0; newMetadata = true; } else { + signal.numMissedPackets = 0; for (const auto& [key, vector] : externalSignalCollection->vectors) { signalName = signal.signalName + TREE_DELIM + key; @@ -1599,6 +1601,15 @@ void YarpRobotLoggerDevice::run() } } + else if (externalSignalCollection == nullptr && signal.connected) + { + signal.numMissedPackets++; + if (signal.numMissedPackets == maxTimoutForExogenousSignal) + { + signal.connected = false; + signal.numMissedPackets = 0; + } + } } Eigen::Matrix newMetadataVector; if(newMetadata) From 0e166b4374037573ecec45fbbb5a55c231fc08d9 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 22 Feb 2024 09:28:31 +0100 Subject: [PATCH 60/83] Removed bug regarding metadata --- src/YarpUtilities/src/VectorsCollectionServer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/YarpUtilities/src/VectorsCollectionServer.cpp b/src/YarpUtilities/src/VectorsCollectionServer.cpp index 1b84123318..5168e018b4 100644 --- a/src/YarpUtilities/src/VectorsCollectionServer.cpp +++ b/src/YarpUtilities/src/VectorsCollectionServer.cpp @@ -158,11 +158,11 @@ bool VectorsCollectionServer::populateData(const std::string& key, constexpr auto logPrefix = "[VectorsCollectionServer::setData]"; // check if the metadata has been finalized - if (!m_pimpl->isMetadataFinalized) - { - log()->error("{} The metadata has not been finalized.", logPrefix); - return false; - } + // if (!m_pimpl->isMetadataFinalized) + // { + // log()->error("{} The metadata has not been finalized.", logPrefix); + // return false; + // } // check if the key exists if (m_pimpl->setOfKeys.find(key) == m_pimpl->setOfKeys.end()) From 107860c462933616e87badf82a0fe08c514616a2 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 29 Feb 2024 10:38:57 +0100 Subject: [PATCH 61/83] changed function name to snake case --- bindings/python/YarpUtilities/src/VectorsCollection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index 3dd1a101e3..7ef2985e0f 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -104,7 +104,7 @@ void CreateVectorsCollectionMetadata(pybind11::module& module) [](const VectorsCollectionMetadata& impl, yarp::os::ConnectionWriter& connection) -> bool { return impl.write(connection); }) - .def("toString", &VectorsCollectionMetadata::toString) + .def("to_string", &VectorsCollectionMetadata::toString) .def("getVectors", [](const VectorsCollectionMetadata& impl) -> std::map> { return impl.vectors; From 26f7a3a5a5f7bc8859377a62891a13b9bf73c94f Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 29 Feb 2024 10:40:23 +0100 Subject: [PATCH 62/83] Updated the git ignore --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 9725c751a9..98fcf6d05d 100644 --- a/.gitignore +++ b/.gitignore @@ -155,3 +155,6 @@ dmypy.json # Cython debug symbols cython_debug/ + +# compile commands file for resolving includes +compile_commands.json From 0392a0409591e211c09e5a39379010ff81c7043e Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 29 Feb 2024 10:46:18 +0100 Subject: [PATCH 63/83] Replaced yInfo with log()->info() --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 56027a9a05..90f67d3a35 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -135,7 +135,7 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) sendDataRT = params->getParameter("remote", remote); if (sendDataRT) { - yInfo() << "Activating Real Time Logging on yarp port: " << remote; + log()->info("{} Activating Real Time Logging on yarp port: {}", logPrefix, remote); initMetadataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::addChannelAndMetadata; loggingDataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::storeAndSendLoggingData; if (!m_vectorCollectionRTDataServer.initialize(params)) @@ -146,7 +146,7 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) } else { - yInfo() << "Real time logging not activated"; + log()->info("{} Real time logging not activated", logPrefix); initMetadataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::addChannel; loggingDataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::storeLoggingData; } From cf0370bd7a167d2ce02711edb93411a13482fe64 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 29 Feb 2024 11:06:02 +0100 Subject: [PATCH 64/83] Fixed formatting --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 4 ++++ src/YarpUtilities/src/VectorsCollectionServer.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 90f67d3a35..53e25c0445 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -874,7 +874,9 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) m_vectorCollectionRTDataServer.populateMetadata(ROBOT_RT_ROOT_NAME + TREE_DELIM + "newMetadata", {"newMetadata"}); if(sendDataRT) + { m_vectorCollectionRTDataServer.finalizeMetadata(); + } // resize the temporary vectors m_jointSensorBuffer.resize(dofs); @@ -1669,7 +1671,9 @@ void YarpRobotLoggerDevice::run() } // end of lock guard scope for text logging port if(sendDataRT) + { m_vectorCollectionRTDataServer.sendData(); + } } bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, diff --git a/src/YarpUtilities/src/VectorsCollectionServer.cpp b/src/YarpUtilities/src/VectorsCollectionServer.cpp index 5168e018b4..e70a89f5ed 100644 --- a/src/YarpUtilities/src/VectorsCollectionServer.cpp +++ b/src/YarpUtilities/src/VectorsCollectionServer.cpp @@ -106,7 +106,7 @@ bool VectorsCollectionServer::populateMetadata(const std::string& key, // return false; // } - // // check if the key has been already used + // check if the key has been already used if (m_pimpl->setOfKeys.find(key) != m_pimpl->setOfKeys.end()) { log()->error("[VectorsCollectionServer::populateMetadata] The key {} has been already " From 70e0b6f947d47e199e64e0d896593c1b340f0a83 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 29 Feb 2024 11:50:46 +0100 Subject: [PATCH 65/83] Fixed variable names --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 69 ++++----- .../src/YarpRobotLoggerDevice.cpp | 144 +++++++++--------- 2 files changed, 107 insertions(+), 106 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 4ad5157507..82bb5e9db8 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -65,7 +65,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::unique_ptr m_robotSensorBridge; std::unique_ptr m_cameraBridge; - bool sendDataRT; + bool m_sendDataRT; BipedalLocomotion::YarpUtilities::VectorsCollectionServer m_vectorCollectionRTDataServer; template struct ExogenousSignal @@ -217,57 +217,58 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const unsigned int maxTimoutForExogenousSignal = 10; - const std::string TREE_DELIM = "::"; + const std::string treeDelim = "::"; - const std::string ROBOT_RT_ROOT_NAME = "robot_realtime"; + const std::string robotRtRootName = "robot_realtime"; - const std::string JOINT_STATE_POSITIONS_NAME = "joints_state::positions"; - const std::string JOINT_STATE_VELOCITIES_NAME = "joints_state::velocities"; - const std::string JOINT_STATE_ACCLERATIONS_NAME = "joints_state::accelerations"; - const std::string JOINT_STATE_TORQUES_NAME = "joints_state::torques"; + const std::string jointStatePositionsName = "joints_state::positions"; + const std::string jointStateVelocitiesName = "joints_state::velocities"; + const std::string jointStateAccelerationsName = "joints_state::accelerations"; + const std::string jointStateTorquesName = "joints_state::torques"; - const std::string MOTOR_STATE_POSITIONS_NAME = "motors_state::positions"; - const std::string MOTOR_STATE_VELOCITIES_NAME = "motors_state::velocities"; - const std::string MOTOR_STATE_ACCELERATIONS_NAME = "motors_state::acclerations"; - const std::string MOTOR_STATE_CURRENTS_NAME = "motors_state::currents"; - const std::string MOTOR_STATE_PWM_NAME = "motors_state::PWM"; + const std::string motorStatePositionsName = "motors_state::positions"; + const std::string motorStateVelocitiesName = "motors_state::velocities"; + const std::string motorStateAccelerationsName = "motors_state::accelerations"; + const std::string motorStateCurrentsName = "motors_state::currents"; + const std::string motorStatePwmName = "motors_state::PWM"; - const std::string MOTOR_STATE_PIDS_NAME = "PIDs"; + const std::string motorStatePidsName = "PIDs"; - const std::string FTS_NAME = "FTs"; + const std::string ftsName = "FTs"; - const std::vector FTElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; + const std::vector ftElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; - const std::string GYROS_NAME = "gyros"; - const std::vector GyroElementNames = {"omega_x", "omega_y", "omega_z"}; + const std::string gyrosName = "gyros"; + const std::vector gyroElementNames = {"omega_x", "omega_y", "omega_z"}; - const std::string ACCELEROMETERS_NAME = "accelerometers"; + const std::string accelerometersName = "accelerometers"; const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; - const std::string ORIENTATIONS_NAME = "orientations"; - const std::vector OrientationElementNames = {"r", "p", "y"}; + const std::string orientationsName = "orientations"; + const std::vector orientationElementNames = {"r", "p", "y"}; - const std::string MAGNETOMETERS_NAME = "magnetometers"; - const std::vector MagnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; + const std::string magnetometersName = "magnetometers"; + const std::vector magnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; - const std::string CARTESIAN_WRENCHES_NAME = "cartesian_wrenches"; - const std::vector CartesianWrenchNames = {FTElementNames[0], - FTElementNames[1], - FTElementNames[2], - FTElementNames[3], - FTElementNames[4], - FTElementNames[5]}; + const std::string cartesianWrenchesName = "cartesian_wrenches"; + const std::vector cartesianWrenchNames = {ftElementNames[0], + ftElementNames[1], + ftElementNames[2], + ftElementNames[3], + ftElementNames[4], + ftElementNames[5]}; - const std::string TEMPERATURE_NAME = "temperatures"; - const std::vector TemperatureNames = {"temperature"}; + const std::string temperatureName = "temperatures"; + const std::vector temperatureNames = {"temperature"}; - const std::string YARP_NAME = "yarp_robot_name"; + const std::string robotName = "yarp_robot_name"; - const std::string ROBOT_DESCRIPTON_LIST = "description_list"; + const std::string robotDescriptionList = "description_list"; - const std::string TIMESTAMPS_NAME = "timestamps"; + const std::string timestampsName = "timestamps"; }; } // namespace BipedalLocomotion + #endif // BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_H diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 53e25c0445..ab510ef13b 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -106,7 +106,7 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, std::make_shared()); m_updatedMetadataSignalVal = 0; - sendDataRT = false; + m_sendDataRT = false; } YarpRobotLoggerDevice::YarpRobotLoggerDevice() @@ -121,7 +121,7 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() std::make_shared()); m_updatedMetadataSignalVal = 0; - sendDataRT = false; + m_sendDataRT = false; } YarpRobotLoggerDevice::~YarpRobotLoggerDevice() = default; @@ -132,8 +132,8 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) constexpr auto logPrefix = "[YarpRobotLoggerDevice::open]"; auto params = std::make_shared(config); std::string remote; - sendDataRT = params->getParameter("remote", remote); - if (sendDataRT) + m_sendDataRT = params->getParameter("remote", remote); + if (m_sendDataRT) { log()->info("{} Activating Real Time Logging on yarp port: {}", logPrefix, remote); initMetadataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::addChannelAndMetadata; @@ -636,7 +636,7 @@ bool YarpRobotLoggerDevice::addChannelAndMetadata(const std::string& nameKey, const std::vector& metadataNames) { bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames}); - std::string rtNameKey = ROBOT_RT_ROOT_NAME + TREE_DELIM + nameKey; + std::string rtNameKey = robotRtRootName + treeDelim + nameKey; ok &= m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames); return ok; } @@ -646,7 +646,7 @@ void YarpRobotLoggerDevice::storeAndSendLoggingData(const std::string& name, const double time) { m_bufferManager.push_back(data, time, name); - std::string rtName = ROBOT_RT_ROOT_NAME + TREE_DELIM + name; + std::string rtName = robotRtRootName + treeDelim + name; m_vectorCollectionRTDataServer.populateData(rtName, data); } @@ -707,46 +707,46 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) const unsigned dofs = joints.size(); m_bufferManager.setDescriptionList(joints); - if(sendDataRT) + if(m_sendDataRT) { char* tmp = std::getenv("YARP_ROBOT_NAME"); std::string metadataName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + YARP_NAME; + = robotRtRootName + treeDelim + robotName; m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); metadataName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + TIMESTAMPS_NAME; - m_vectorCollectionRTDataServer.populateMetadata(metadataName, {TIMESTAMPS_NAME}); + = robotRtRootName + treeDelim + timestampsName; + m_vectorCollectionRTDataServer.populateMetadata(metadataName, {timestampsName}); - std::string rtMetadataName = ROBOT_RT_ROOT_NAME + TREE_DELIM - + ROBOT_DESCRIPTON_LIST; + std::string rtMetadataName = robotRtRootName + treeDelim + + robotDescriptionList; m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); } // prepare the telemetry if (m_streamJointStates) { - ok &= (*this.*initMetadataFunction)(JOINT_STATE_POSITIONS_NAME, joints); - ok &= (*this.*initMetadataFunction)(JOINT_STATE_VELOCITIES_NAME, joints); - ok &= (*this.*initMetadataFunction)(JOINT_STATE_ACCLERATIONS_NAME, joints); - ok &= (*this.*initMetadataFunction)(JOINT_STATE_TORQUES_NAME, joints); + ok &= (*this.*initMetadataFunction)(jointStatePositionsName, joints); + ok &= (*this.*initMetadataFunction)(jointStateVelocitiesName, joints); + ok &= (*this.*initMetadataFunction)(jointStateAccelerationsName, joints); + ok &= (*this.*initMetadataFunction)(jointStateTorquesName, joints); } if (m_streamMotorStates) { - ok &= (*this.*initMetadataFunction)(MOTOR_STATE_POSITIONS_NAME, joints); - ok &= (*this.*initMetadataFunction)(MOTOR_STATE_VELOCITIES_NAME, joints); - ok &= (*this.*initMetadataFunction)(MOTOR_STATE_ACCELERATIONS_NAME, joints); - ok &= (*this.*initMetadataFunction)(MOTOR_STATE_CURRENTS_NAME, joints); + ok &= (*this.*initMetadataFunction)(motorStatePositionsName, joints); + ok &= (*this.*initMetadataFunction)(motorStateVelocitiesName, joints); + ok &= (*this.*initMetadataFunction)(motorStateAccelerationsName, joints); + ok &= (*this.*initMetadataFunction)(motorStateCurrentsName, joints); } if (m_streamMotorPWM) { - ok &= (*this.*initMetadataFunction)(MOTOR_STATE_PWM_NAME, joints); + ok &= (*this.*initMetadataFunction)(motorStatePwmName, joints); } if (m_streamPIDs) { - ok &= (*this.*initMetadataFunction)(MOTOR_STATE_PIDS_NAME, joints); + ok &= (*this.*initMetadataFunction)(motorStatePidsName, joints); } if (m_streamFTSensors) @@ -754,8 +754,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { std::string fullFTSensorName - = FTS_NAME + TREE_DELIM + sensorName; - ok &= (*this.*initMetadataFunction)(fullFTSensorName, FTElementNames); + = ftsName + treeDelim + sensorName; + ok &= (*this.*initMetadataFunction)(fullFTSensorName, ftElementNames); } } @@ -764,43 +764,43 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { std::string fullGyroSensorName - = GYROS_NAME + TREE_DELIM + sensorName; - ok &= (*this.*initMetadataFunction)(fullGyroSensorName, GyroElementNames); + = gyrosName + treeDelim + sensorName; + ok &= (*this.*initMetadataFunction)(fullGyroSensorName, gyroElementNames); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { std::string fullAccelerometerSensorName - = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; + = accelerometersName + treeDelim + sensorName; ok &= (*this.*initMetadataFunction)(fullAccelerometerSensorName, AccelerometerElementNames); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { std::string fullOrientationsSensorName - = ORIENTATIONS_NAME + TREE_DELIM + sensorName; - ok &= (*this.*initMetadataFunction)(fullOrientationsSensorName, OrientationElementNames); + = orientationsName + treeDelim + sensorName; + ok &= (*this.*initMetadataFunction)(fullOrientationsSensorName, orientationElementNames); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { std::string fullMagnetometersSensorName - = MAGNETOMETERS_NAME + TREE_DELIM + sensorName; - ok &= (*this.*initMetadataFunction)(fullMagnetometersSensorName, MagnetometerElementNames); + = magnetometersName + treeDelim + sensorName; + ok &= (*this.*initMetadataFunction)(fullMagnetometersSensorName, magnetometerElementNames); } // an IMU contains a gyro accelerometer and an orientation sensor for (const auto& sensorName : m_robotSensorBridge->getIMUsList()) { std::string fullAccelerometerSensorName - = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; + = accelerometersName + treeDelim + sensorName; std::string fullGyroSensorName - = GYROS_NAME + TREE_DELIM + sensorName; + = gyrosName + treeDelim + sensorName; std::string fullOrientationsSensorName - = ORIENTATIONS_NAME + TREE_DELIM + sensorName; + = orientationsName + treeDelim + sensorName; ok &= (*this.*initMetadataFunction)(fullAccelerometerSensorName, AccelerometerElementNames); - ok &= (*this.*initMetadataFunction)(fullGyroSensorName, GyroElementNames); - ok &= (*this.*initMetadataFunction)(fullOrientationsSensorName, OrientationElementNames); + ok &= (*this.*initMetadataFunction)(fullGyroSensorName, gyroElementNames); + ok &= (*this.*initMetadataFunction)(fullOrientationsSensorName, orientationElementNames); } } @@ -808,9 +808,9 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { - std::string fullCartesianWrenchName = CARTESIAN_WRENCHES_NAME - + TREE_DELIM + cartesianWrenchName; - ok &= (*this.*initMetadataFunction)(fullCartesianWrenchName, CartesianWrenchNames); + std::string fullCartesianWrenchName = cartesianWrenchesName + + treeDelim + cartesianWrenchName; + ok &= (*this.*initMetadataFunction)(fullCartesianWrenchName, cartesianWrenchNames); } } @@ -819,8 +819,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { std::string fullTemperatureSensorName - = TEMPERATURE_NAME + TREE_DELIM + sensorName; - ok &= (*this.*initMetadataFunction)(fullTemperatureSensorName, TemperatureNames); + = temperatureName + treeDelim + sensorName; + ok &= (*this.*initMetadataFunction)(fullTemperatureSensorName, temperatureNames); } } @@ -838,7 +838,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& [key, vector] : externalSignalCollection->vectors) { - signalName = signal.signalName + TREE_DELIM + key; + signalName = signal.signalName + treeDelim + key; const auto& metadata = signal.metadata.vectors.find(key); if (metadata == signal.metadata.vectors.cend()) { @@ -871,9 +871,9 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } } - m_vectorCollectionRTDataServer.populateMetadata(ROBOT_RT_ROOT_NAME + TREE_DELIM + "newMetadata", {"newMetadata"}); + m_vectorCollectionRTDataServer.populateMetadata(robotRtRootName + treeDelim + "newMetadata", {"newMetadata"}); - if(sendDataRT) + if(m_sendDataRT) { m_vectorCollectionRTDataServer.finalizeMetadata(); } @@ -1375,14 +1375,14 @@ void YarpRobotLoggerDevice::run() std::string rtSignalName = ""; std::lock_guard lock(m_bufferManagerMutex); - if(sendDataRT) + if(m_sendDataRT) { m_vectorCollectionRTDataServer.prepareData(); m_vectorCollectionRTDataServer.clearData(); Eigen::Matrix timeData; timeData << time; rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + TIMESTAMPS_NAME; + = robotRtRootName + treeDelim + timestampsName; m_vectorCollectionRTDataServer.populateData(rtSignalName, timeData); } @@ -1396,19 +1396,19 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(JOINT_STATE_POSITIONS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(jointStatePositionsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(JOINT_STATE_VELOCITIES_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(jointStateVelocitiesName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(JOINT_STATE_ACCLERATIONS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(jointStateAccelerationsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(JOINT_STATE_TORQUES_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(jointStateTorquesName, m_jointSensorBuffer, time); } } @@ -1416,19 +1416,19 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(MOTOR_STATE_POSITIONS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(motorStatePositionsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(MOTOR_STATE_VELOCITIES_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(motorStateVelocitiesName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(MOTOR_STATE_ACCELERATIONS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(motorStateAccelerationsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(MOTOR_STATE_CURRENTS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(motorStateCurrentsName, m_jointSensorBuffer, time); } } @@ -1436,7 +1436,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(MOTOR_STATE_PWM_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(motorStatePwmName, m_jointSensorBuffer, time); } } @@ -1444,7 +1444,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(MOTOR_STATE_PIDS_NAME, m_jointSensorBuffer, time); + (*this.*loggingDataFunction)(motorStatePidsName, m_jointSensorBuffer, time); } } @@ -1454,7 +1454,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { - signalName = FTS_NAME + TREE_DELIM + sensorName; + signalName = ftsName + treeDelim + sensorName; (*this.*loggingDataFunction)(signalName, m_ftBuffer, time); } } @@ -1466,7 +1466,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) { - signalName = TEMPERATURE_NAME + TREE_DELIM + sensorName; + signalName = temperatureName + treeDelim + sensorName; Eigen::Matrix temperatureData; temperatureData << m_ftTemperatureBuffer; @@ -1481,7 +1481,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { - signalName = GYROS_NAME + TREE_DELIM + sensorName; + signalName = gyrosName + treeDelim + sensorName; (*this.*loggingDataFunction)(signalName, m_gyroBuffer, time); } } @@ -1493,7 +1493,7 @@ void YarpRobotLoggerDevice::run() m_acceloremeterBuffer)) { signalName - = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; + = accelerometersName + treeDelim + sensorName; (*this.*loggingDataFunction)(signalName, m_acceloremeterBuffer, time); } } @@ -1504,7 +1504,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) { - signalName = ORIENTATIONS_NAME + TREE_DELIM + sensorName; + signalName = orientationsName + treeDelim + sensorName; (*this.*loggingDataFunction)(signalName, m_orientationBuffer, time); } } @@ -1513,7 +1513,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { - signalName = MAGNETOMETERS_NAME + TREE_DELIM + sensorName; + signalName = magnetometersName + treeDelim + sensorName; (*this.*loggingDataFunction)(signalName, m_magnemetometerBuffer, time); } } @@ -1530,13 +1530,13 @@ void YarpRobotLoggerDevice::run() m_gyroBuffer, m_orientationBuffer); - signalName = ACCELEROMETERS_NAME + TREE_DELIM + sensorName; + signalName = accelerometersName + treeDelim + sensorName; (*this.*loggingDataFunction)(signalName, m_acceloremeterBuffer, time); - signalName = GYROS_NAME + TREE_DELIM + sensorName; + signalName = gyrosName + treeDelim + sensorName; (*this.*loggingDataFunction)(signalName, m_gyroBuffer, time); - signalName = ORIENTATIONS_NAME + TREE_DELIM + sensorName; + signalName = orientationsName + treeDelim + sensorName; (*this.*loggingDataFunction)(signalName, m_orientationBuffer, time); } } @@ -1547,7 +1547,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) { - signalName = CARTESIAN_WRENCHES_NAME + TREE_DELIM + signalName = cartesianWrenchesName + treeDelim + cartesianWrenchName; (*this.*loggingDataFunction)(signalName, m_ftBuffer, time); } @@ -1568,7 +1568,7 @@ void YarpRobotLoggerDevice::run() { for (const auto& [key, vector] : externalSignalCollection->vectors) { - signalName = signal.signalName + TREE_DELIM + key; + signalName = signal.signalName + treeDelim + key; const auto& metadata = signal.metadata.vectors.find(key); if (metadata == signal.metadata.vectors.cend()) { @@ -1591,12 +1591,12 @@ void YarpRobotLoggerDevice::run() signal.numMissedPackets = 0; for (const auto& [key, vector] : externalSignalCollection->vectors) { - signalName = signal.signalName + TREE_DELIM + key; + signalName = signal.signalName + treeDelim + key; m_bufferManager.push_back(vector, time, signalName); - if(sendDataRT) + if(m_sendDataRT) { rtSignalName - = ROBOT_RT_ROOT_NAME + TREE_DELIM + signalName; + = robotRtRootName + treeDelim + signalName; m_vectorCollectionRTDataServer.populateData(rtSignalName, vector); } } @@ -1618,7 +1618,7 @@ void YarpRobotLoggerDevice::run() m_updatedMetadataSignalVal = (m_updatedMetadataSignalVal + 1) % 2; newMetadataVector << m_updatedMetadataSignalVal; - m_vectorCollectionRTDataServer.populateData(ROBOT_RT_ROOT_NAME + TREE_DELIM + "newMetadata", newMetadataVector); + m_vectorCollectionRTDataServer.populateData(robotRtRootName + treeDelim + "newMetadata", newMetadataVector); for (auto& [name, signal] : m_vectorSignals) { @@ -1670,7 +1670,7 @@ void YarpRobotLoggerDevice::run() } } // end of lock guard scope for text logging port - if(sendDataRT) + if(m_sendDataRT) { m_vectorCollectionRTDataServer.sendData(); } From 3efcfe5c3bd139658f7d9f0fee62a00c44fb8fce Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Mon, 4 Mar 2024 14:18:04 +0100 Subject: [PATCH 66/83] Updated max timeout for exogenous signals --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 82bb5e9db8..3e9a80cf1e 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -215,7 +215,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string& camera, const std::string& imageType); - const unsigned int maxTimoutForExogenousSignal = 10; + const unsigned int maxTimoutForExogenousSignal = 1000; const std::string treeDelim = "::"; From 3f9d72533877a3be5136f55da086e944f05ab733 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Mon, 4 Mar 2024 15:09:09 +0100 Subject: [PATCH 67/83] Removed complexity for a simpler PR --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 16 +- .../src/YarpRobotLoggerDevice.cpp | 142 +++++++----------- .../src/VectorsCollectionServer.cpp | 22 +-- 3 files changed, 69 insertions(+), 111 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 3e9a80cf1e..54e2a452d3 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -109,7 +109,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::unordered_map m_vectorsCollectionSignals; std::unordered_map> m_vectorSignals; - unsigned int m_updatedMetadataSignalVal; std::unordered_set m_exogenousPortsStoredInManager; std::atomic m_lookForNewExogenousSignalIsRunning{false}; @@ -180,17 +179,8 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void lookForNewLogs(); void lookForExogenousSignals(); - bool - addChannelAndMetadata(const std::string& nameKey, const std::vector& metadata); - void storeAndSendLoggingData(const std::string& name, - const Eigen::VectorXd& data, - const double time); - bool addChannel(const std::string& nameKey, const std::vector& metadata); - void storeLoggingData(const std::string& name, - const Eigen::VectorXd& data, - const double time); - bool (BipedalLocomotion::YarpRobotLoggerDevice::*initMetadataFunction)(const std::string& nameKey, const std::vector& metadata); - void (BipedalLocomotion::YarpRobotLoggerDevice::*loggingDataFunction)(const std::string& name, + bool initMetadata(const std::string& nameKey, const std::vector& metadata); + void logData(const std::string& name, const Eigen::VectorXd& data, const double time); @@ -215,8 +205,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string& camera, const std::string& imageType); - const unsigned int maxTimoutForExogenousSignal = 1000; - const std::string treeDelim = "::"; const std::string robotRtRootName = "robot_realtime"; diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index ab510ef13b..f68a2ce0af 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -105,7 +105,6 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); - m_updatedMetadataSignalVal = 0; m_sendDataRT = false; } @@ -120,7 +119,6 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); - m_updatedMetadataSignalVal = 0; m_sendDataRT = false; } @@ -136,8 +134,6 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) if (m_sendDataRT) { log()->info("{} Activating Real Time Logging on yarp port: {}", logPrefix, remote); - initMetadataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::addChannelAndMetadata; - loggingDataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::storeAndSendLoggingData; if (!m_vectorCollectionRTDataServer.initialize(params)) { log()->error("Failed to initalize the vectorsCollectionServer", logPrefix); @@ -147,8 +143,6 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) else { log()->info("{} Real time logging not activated", logPrefix); - initMetadataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::addChannel; - loggingDataFunction = &BipedalLocomotion::YarpRobotLoggerDevice::storeLoggingData; } double devicePeriod{0.01}; @@ -632,36 +626,27 @@ bool YarpRobotLoggerDevice::setupRobotCameraBridge( return true; } -bool YarpRobotLoggerDevice::addChannelAndMetadata(const std::string& nameKey, - const std::vector& metadataNames) +bool YarpRobotLoggerDevice::initMetadata(const std::string& nameKey, const std::vector& metadataNames) { bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames}); std::string rtNameKey = robotRtRootName + treeDelim + nameKey; - ok &= m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames); + if (m_sendDataRT) + { + ok &= m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames); + } return ok; } -void YarpRobotLoggerDevice::storeAndSendLoggingData(const std::string& name, +void YarpRobotLoggerDevice::logData(const std::string& name, const Eigen::VectorXd& data, const double time) { m_bufferManager.push_back(data, time, name); std::string rtName = robotRtRootName + treeDelim + name; - m_vectorCollectionRTDataServer.populateData(rtName, data); -} - -bool YarpRobotLoggerDevice::addChannel(const std::string& nameKey, - const std::vector& metadataNames) -{ - bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames}); - return ok; -} - -void YarpRobotLoggerDevice::storeLoggingData(const std::string& name, - const Eigen::VectorXd& data, - const double time) -{ - m_bufferManager.push_back(data, time, name); + if(m_sendDataRT) + { + m_vectorCollectionRTDataServer.populateData(rtName, data); + } } bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) @@ -726,27 +711,27 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // prepare the telemetry if (m_streamJointStates) { - ok &= (*this.*initMetadataFunction)(jointStatePositionsName, joints); - ok &= (*this.*initMetadataFunction)(jointStateVelocitiesName, joints); - ok &= (*this.*initMetadataFunction)(jointStateAccelerationsName, joints); - ok &= (*this.*initMetadataFunction)(jointStateTorquesName, joints); + ok &= initMetadata(jointStatePositionsName, joints); + ok &= initMetadata(jointStateVelocitiesName, joints); + ok &= initMetadata(jointStateAccelerationsName, joints); + ok &= initMetadata(jointStateTorquesName, joints); } if (m_streamMotorStates) { - ok &= (*this.*initMetadataFunction)(motorStatePositionsName, joints); - ok &= (*this.*initMetadataFunction)(motorStateVelocitiesName, joints); - ok &= (*this.*initMetadataFunction)(motorStateAccelerationsName, joints); - ok &= (*this.*initMetadataFunction)(motorStateCurrentsName, joints); + ok &= initMetadata(motorStatePositionsName, joints); + ok &= initMetadata(motorStateVelocitiesName, joints); + ok &= initMetadata(motorStateAccelerationsName, joints); + ok &= initMetadata(motorStateCurrentsName, joints); } if (m_streamMotorPWM) { - ok &= (*this.*initMetadataFunction)(motorStatePwmName, joints); + ok &= initMetadata(motorStatePwmName, joints); } if (m_streamPIDs) { - ok &= (*this.*initMetadataFunction)(motorStatePidsName, joints); + ok &= initMetadata(motorStatePidsName, joints); } if (m_streamFTSensors) @@ -755,7 +740,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullFTSensorName = ftsName + treeDelim + sensorName; - ok &= (*this.*initMetadataFunction)(fullFTSensorName, ftElementNames); + ok &= initMetadata(fullFTSensorName, ftElementNames); } } @@ -765,28 +750,28 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullGyroSensorName = gyrosName + treeDelim + sensorName; - ok &= (*this.*initMetadataFunction)(fullGyroSensorName, gyroElementNames); + ok &= initMetadata(fullGyroSensorName, gyroElementNames); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { std::string fullAccelerometerSensorName = accelerometersName + treeDelim + sensorName; - ok &= (*this.*initMetadataFunction)(fullAccelerometerSensorName, AccelerometerElementNames); + ok &= initMetadata(fullAccelerometerSensorName, AccelerometerElementNames); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { std::string fullOrientationsSensorName = orientationsName + treeDelim + sensorName; - ok &= (*this.*initMetadataFunction)(fullOrientationsSensorName, orientationElementNames); + ok &= initMetadata(fullOrientationsSensorName, orientationElementNames); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { std::string fullMagnetometersSensorName = magnetometersName + treeDelim + sensorName; - ok &= (*this.*initMetadataFunction)(fullMagnetometersSensorName, magnetometerElementNames); + ok &= initMetadata(fullMagnetometersSensorName, magnetometerElementNames); } // an IMU contains a gyro accelerometer and an orientation sensor @@ -798,9 +783,9 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) = gyrosName + treeDelim + sensorName; std::string fullOrientationsSensorName = orientationsName + treeDelim + sensorName; - ok &= (*this.*initMetadataFunction)(fullAccelerometerSensorName, AccelerometerElementNames); - ok &= (*this.*initMetadataFunction)(fullGyroSensorName, gyroElementNames); - ok &= (*this.*initMetadataFunction)(fullOrientationsSensorName, orientationElementNames); + ok &= initMetadata(fullAccelerometerSensorName, AccelerometerElementNames); + ok &= initMetadata(fullGyroSensorName, gyroElementNames); + ok &= initMetadata(fullOrientationsSensorName, orientationElementNames); } } @@ -810,7 +795,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullCartesianWrenchName = cartesianWrenchesName + treeDelim + cartesianWrenchName; - ok &= (*this.*initMetadataFunction)(fullCartesianWrenchName, cartesianWrenchNames); + ok &= initMetadata(fullCartesianWrenchName, cartesianWrenchNames); } } @@ -820,7 +805,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullTemperatureSensorName = temperatureName + treeDelim + sensorName; - ok &= (*this.*initMetadataFunction)(fullTemperatureSensorName, temperatureNames); + ok &= initMetadata(fullTemperatureSensorName, temperatureNames); } } @@ -846,10 +831,10 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) "default one will be used.", logPrefix, signalName); - (*this.*initMetadataFunction)(signalName, {}); + initMetadata(signalName, {}); } else { - (*this.*initMetadataFunction)(signalName, {metadata->second}); + initMetadata(signalName, {metadata->second}); } } signal.dataArrived = true; @@ -865,7 +850,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { if (!signal.dataArrived) { - (*this.*initMetadataFunction)(signalName, {}); + initMetadata(signalName, {}); signal.dataArrived = true; } } @@ -1396,19 +1381,19 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(jointStatePositionsName, m_jointSensorBuffer, time); + logData(jointStatePositionsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(jointStateVelocitiesName, m_jointSensorBuffer, time); + logData(jointStateVelocitiesName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(jointStateAccelerationsName, m_jointSensorBuffer, time); + logData(jointStateAccelerationsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(jointStateTorquesName, m_jointSensorBuffer, time); + logData(jointStateTorquesName, m_jointSensorBuffer, time); } } @@ -1416,19 +1401,19 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(motorStatePositionsName, m_jointSensorBuffer, time); + logData(motorStatePositionsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(motorStateVelocitiesName, m_jointSensorBuffer, time); + logData(motorStateVelocitiesName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(motorStateAccelerationsName, m_jointSensorBuffer, time); + logData(motorStateAccelerationsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(motorStateCurrentsName, m_jointSensorBuffer, time); + logData(motorStateCurrentsName, m_jointSensorBuffer, time); } } @@ -1436,7 +1421,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(motorStatePwmName, m_jointSensorBuffer, time); + logData(motorStatePwmName, m_jointSensorBuffer, time); } } @@ -1444,7 +1429,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) { - (*this.*loggingDataFunction)(motorStatePidsName, m_jointSensorBuffer, time); + logData(motorStatePidsName, m_jointSensorBuffer, time); } } @@ -1455,7 +1440,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { signalName = ftsName + treeDelim + sensorName; - (*this.*loggingDataFunction)(signalName, m_ftBuffer, time); + logData(signalName, m_ftBuffer, time); } } } @@ -1470,7 +1455,7 @@ void YarpRobotLoggerDevice::run() Eigen::Matrix temperatureData; temperatureData << m_ftTemperatureBuffer; - (*this.*loggingDataFunction)(signalName, temperatureData, time); + logData(signalName, temperatureData, time); } } } @@ -1482,7 +1467,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { signalName = gyrosName + treeDelim + sensorName; - (*this.*loggingDataFunction)(signalName, m_gyroBuffer, time); + logData(signalName, m_gyroBuffer, time); } } @@ -1494,7 +1479,7 @@ void YarpRobotLoggerDevice::run() { signalName = accelerometersName + treeDelim + sensorName; - (*this.*loggingDataFunction)(signalName, m_acceloremeterBuffer, time); + logData(signalName, m_acceloremeterBuffer, time); } } @@ -1505,7 +1490,7 @@ void YarpRobotLoggerDevice::run() m_orientationBuffer)) { signalName = orientationsName + treeDelim + sensorName; - (*this.*loggingDataFunction)(signalName, m_orientationBuffer, time); + logData(signalName, m_orientationBuffer, time); } } @@ -1514,7 +1499,7 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { signalName = magnetometersName + treeDelim + sensorName; - (*this.*loggingDataFunction)(signalName, m_magnemetometerBuffer, time); + logData(signalName, m_magnemetometerBuffer, time); } } } @@ -1531,13 +1516,13 @@ void YarpRobotLoggerDevice::run() m_orientationBuffer); signalName = accelerometersName + treeDelim + sensorName; - (*this.*loggingDataFunction)(signalName, m_acceloremeterBuffer, time); + logData(signalName, m_acceloremeterBuffer, time); signalName = gyrosName + treeDelim + sensorName; - (*this.*loggingDataFunction)(signalName, m_gyroBuffer, time); + logData(signalName, m_gyroBuffer, time); signalName = orientationsName + treeDelim + sensorName; - (*this.*loggingDataFunction)(signalName, m_orientationBuffer, time); + logData(signalName, m_orientationBuffer, time); } } @@ -1549,7 +1534,7 @@ void YarpRobotLoggerDevice::run() { signalName = cartesianWrenchesName + treeDelim + cartesianWrenchName; - (*this.*loggingDataFunction)(signalName, m_ftBuffer, time); + logData(signalName, m_ftBuffer, time); } } } @@ -1576,10 +1561,10 @@ void YarpRobotLoggerDevice::run() "default one will be used.", logPrefix, signalName); - (*this.*initMetadataFunction)(signalName, {}); + initMetadata(signalName, {}); } else { - (*this.*initMetadataFunction)(signalName, {metadata->second}); + initMetadata(signalName, {metadata->second}); } } signal.dataArrived = true; @@ -1603,22 +1588,7 @@ void YarpRobotLoggerDevice::run() } } - else if (externalSignalCollection == nullptr && signal.connected) - { - signal.numMissedPackets++; - if (signal.numMissedPackets == maxTimoutForExogenousSignal) - { - signal.connected = false; - signal.numMissedPackets = 0; - } - } } - Eigen::Matrix newMetadataVector; - if(newMetadata) - m_updatedMetadataSignalVal = (m_updatedMetadataSignalVal + 1) % 2; - newMetadataVector << m_updatedMetadataSignalVal; - - m_vectorCollectionRTDataServer.populateData(robotRtRootName + treeDelim + "newMetadata", newMetadataVector); for (auto& [name, signal] : m_vectorSignals) { @@ -1628,7 +1598,7 @@ void YarpRobotLoggerDevice::run() { if (!signal.dataArrived) { - (*this.*initMetadataFunction)(signalName, {}); + initMetadata(signalName, {}); signal.dataArrived = true; } } diff --git a/src/YarpUtilities/src/VectorsCollectionServer.cpp b/src/YarpUtilities/src/VectorsCollectionServer.cpp index e70a89f5ed..5a7e82027f 100644 --- a/src/YarpUtilities/src/VectorsCollectionServer.cpp +++ b/src/YarpUtilities/src/VectorsCollectionServer.cpp @@ -99,12 +99,12 @@ bool VectorsCollectionServer::populateMetadata(const std::string& key, // This code was temporarily removed via commenting it out, this will be discussed with the team // later on how to append metadata after it is finalized - // if (m_pimpl->isMetadataFinalized) - // { - // log()->error("[VectorsCollectionServer::populateMetadata] The metadata has been already " - // "populated."); - // return false; - // } + if (m_pimpl->isMetadataFinalized) + { + log()->error("[VectorsCollectionServer::populateMetadata] The metadata has been already " + "populated."); + return false; + } // check if the key has been already used if (m_pimpl->setOfKeys.find(key) != m_pimpl->setOfKeys.end()) @@ -158,11 +158,11 @@ bool VectorsCollectionServer::populateData(const std::string& key, constexpr auto logPrefix = "[VectorsCollectionServer::setData]"; // check if the metadata has been finalized - // if (!m_pimpl->isMetadataFinalized) - // { - // log()->error("{} The metadata has not been finalized.", logPrefix); - // return false; - // } + if (!m_pimpl->isMetadataFinalized) + { + log()->error("{} The metadata has not been finalized.", logPrefix); + return false; + } // check if the key exists if (m_pimpl->setOfKeys.find(key) == m_pimpl->setOfKeys.end()) From e392e9323bc569efa3ef6ded3422322a1a4ab8c3 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Mon, 4 Mar 2024 15:33:01 +0100 Subject: [PATCH 68/83] Further simplified --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 2 -- .../YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 7 ------- 2 files changed, 9 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 54e2a452d3..89f70d1885 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -11,7 +11,6 @@ #include #include #include -#include #include #include #include @@ -99,7 +98,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, BipedalLocomotion::YarpUtilities::VectorsCollectionClient client; BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata metadata; std::string signalName; - unsigned int numMissedPackets; bool dataArrived{false}; bool connected{false}; diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index f68a2ce0af..a36734bbb5 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -856,8 +856,6 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } } - m_vectorCollectionRTDataServer.populateMetadata(robotRtRootName + treeDelim + "newMetadata", {"newMetadata"}); - if(m_sendDataRT) { m_vectorCollectionRTDataServer.finalizeMetadata(); @@ -1539,11 +1537,9 @@ void YarpRobotLoggerDevice::run() } } - bool newMetadata = false; for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); - BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection = signal.client.readData(false); @@ -1568,12 +1564,9 @@ void YarpRobotLoggerDevice::run() } } signal.dataArrived = true; - signal.numMissedPackets = 0; - newMetadata = true; } else { - signal.numMissedPackets = 0; for (const auto& [key, vector] : externalSignalCollection->vectors) { signalName = signal.signalName + treeDelim + key; From ee4cf0dd55ef916922a4013375d9fc9014d13ece Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 12:02:43 +0100 Subject: [PATCH 69/83] Removed leftover comments --- src/YarpUtilities/src/VectorsCollectionServer.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/YarpUtilities/src/VectorsCollectionServer.cpp b/src/YarpUtilities/src/VectorsCollectionServer.cpp index 5a7e82027f..0284760bd2 100644 --- a/src/YarpUtilities/src/VectorsCollectionServer.cpp +++ b/src/YarpUtilities/src/VectorsCollectionServer.cpp @@ -96,9 +96,6 @@ bool VectorsCollectionServer::populateMetadata(const std::string& key, const std::vector& metadata) { // check if the metadata has been already populated - // This code was temporarily removed via commenting it out, this will be discussed with the team - // later on how to append metadata after it is finalized - if (m_pimpl->isMetadataFinalized) { log()->error("[VectorsCollectionServer::populateMetadata] The metadata has been already " From 8f0e4a81bc093986a4829600563a04437575a001 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 12:03:16 +0100 Subject: [PATCH 70/83] replaced unnecessary code with a function --- .../src/YarpRobotLoggerDevice.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index a36734bbb5..8a74f83393 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -1570,13 +1571,8 @@ void YarpRobotLoggerDevice::run() for (const auto& [key, vector] : externalSignalCollection->vectors) { signalName = signal.signalName + treeDelim + key; - m_bufferManager.push_back(vector, time, signalName); - if(m_sendDataRT) - { - rtSignalName - = robotRtRootName + treeDelim + signalName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, vector); - } + Eigen::Matrix eVector(vector.size()); + logData(signalName, eVector, time); } } From 4ea7f85117a66453647b79b9aaf39ffcb1aa6cd5 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 12:26:34 +0100 Subject: [PATCH 71/83] Restructured reading rt setting from log file --- .../YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 8a74f83393..6197d7ab0e 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -130,12 +130,13 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) constexpr auto logPrefix = "[YarpRobotLoggerDevice::open]"; auto params = std::make_shared(config); - std::string remote; - m_sendDataRT = params->getParameter("remote", remote); + auto rtParameters = params->getGroup("REAL_TIME_STREAMING").lock(); + m_sendDataRT = rtParameters != nullptr; if (m_sendDataRT) { + std::string remote; log()->info("{} Activating Real Time Logging on yarp port: {}", logPrefix, remote); - if (!m_vectorCollectionRTDataServer.initialize(params)) + if (!m_vectorCollectionRTDataServer.initialize(rtParameters)) { log()->error("Failed to initalize the vectorsCollectionServer", logPrefix); return false; From f8d16c43c6280982676ab954157a4d7514996c0f Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 13:04:47 +0100 Subject: [PATCH 72/83] Fixed code formatting --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 1 + devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 89f70d1885..4e9ae6a5a4 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -252,6 +252,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string robotDescriptionList = "description_list"; const std::string timestampsName = "timestamps"; + }; } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 6197d7ab0e..c50febb387 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -645,7 +645,7 @@ void YarpRobotLoggerDevice::logData(const std::string& name, { m_bufferManager.push_back(data, time, name); std::string rtName = robotRtRootName + treeDelim + name; - if(m_sendDataRT) + if (m_sendDataRT) { m_vectorCollectionRTDataServer.populateData(rtName, data); } @@ -1547,7 +1547,7 @@ void YarpRobotLoggerDevice::run() if (externalSignalCollection != nullptr) { - if(!signal.dataArrived) + if (!signal.dataArrived) { for (const auto& [key, vector] : externalSignalCollection->vectors) { From 9429b0ef602c66931c08a2682eb245196bb0892f Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 13:05:04 +0100 Subject: [PATCH 73/83] Removed unused includes --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index c50febb387..0ce3700169 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -9,8 +9,6 @@ #include #include #include -#include -#include #include #include #include @@ -19,7 +17,6 @@ #include #include -#include #include #include #include From 69fa835bc1c7725293775adf108c6e5fd53aec7e Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 13:11:22 +0100 Subject: [PATCH 74/83] Fixed more formatting --- .../YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 0ce3700169..bd584276cd 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -691,7 +691,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) const unsigned dofs = joints.size(); m_bufferManager.setDescriptionList(joints); - if(m_sendDataRT) + if (m_sendDataRT) { char* tmp = std::getenv("YARP_ROBOT_NAME"); std::string metadataName @@ -855,7 +855,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } } - if(m_sendDataRT) + if (m_sendDataRT) { m_vectorCollectionRTDataServer.finalizeMetadata(); } @@ -1357,7 +1357,7 @@ void YarpRobotLoggerDevice::run() std::string rtSignalName = ""; std::lock_guard lock(m_bufferManagerMutex); - if(m_sendDataRT) + if (m_sendDataRT) { m_vectorCollectionRTDataServer.prepareData(); m_vectorCollectionRTDataServer.clearData(); @@ -1627,7 +1627,7 @@ void YarpRobotLoggerDevice::run() } } // end of lock guard scope for text logging port - if(m_sendDataRT) + if (m_sendDataRT) { m_vectorCollectionRTDataServer.sendData(); } From 1939bee5c67a86c2fd88821110ecac0611607e81 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 14:40:13 +0100 Subject: [PATCH 75/83] Added use of Eigen::Map --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index bd584276cd..dd69e929ca 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -28,6 +28,8 @@ #include #include +#include + #include #include #include @@ -1569,7 +1571,8 @@ void YarpRobotLoggerDevice::run() for (const auto& [key, vector] : externalSignalCollection->vectors) { signalName = signal.signalName + treeDelim + key; - Eigen::Matrix eVector(vector.size()); + const Eigen::Map> eVector(vector.data(), + vector.size()); logData(signalName, eVector, time); } From 0c6be42cc0c6984ce8322c29753e8ca9d971c4ef Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 14:40:36 +0100 Subject: [PATCH 76/83] Moved the remote string variable --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index dd69e929ca..27bae5811c 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -133,13 +133,14 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) m_sendDataRT = rtParameters != nullptr; if (m_sendDataRT) { - std::string remote; - log()->info("{} Activating Real Time Logging on yarp port: {}", logPrefix, remote); if (!m_vectorCollectionRTDataServer.initialize(rtParameters)) { log()->error("Failed to initalize the vectorsCollectionServer", logPrefix); return false; } + std::string remote; + rtParameters->getParameter("remote", remote); + log()->info("{} Activating Real Time Logging on yarp port: {}", logPrefix, remote); } else { From 7c8d9125c7535a824cb27dc2cb7a8f03bebdff5a Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 14:40:56 +0100 Subject: [PATCH 77/83] Fixed bug with bitwise and --- .../src/YarpRobotLoggerDevice.cpp | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 27bae5811c..ff1d2b3b08 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -634,7 +634,7 @@ bool YarpRobotLoggerDevice::initMetadata(const std::string& nameKey, const std:: std::string rtNameKey = robotRtRootName + treeDelim + nameKey; if (m_sendDataRT) { - ok &= m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames); + ok = ok && m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames); } return ok; } @@ -713,27 +713,27 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) // prepare the telemetry if (m_streamJointStates) { - ok &= initMetadata(jointStatePositionsName, joints); - ok &= initMetadata(jointStateVelocitiesName, joints); - ok &= initMetadata(jointStateAccelerationsName, joints); - ok &= initMetadata(jointStateTorquesName, joints); + ok = ok && initMetadata(jointStatePositionsName, joints); + ok = ok && initMetadata(jointStateVelocitiesName, joints); + ok = ok && initMetadata(jointStateAccelerationsName, joints); + ok = ok && initMetadata(jointStateTorquesName, joints); } if (m_streamMotorStates) { - ok &= initMetadata(motorStatePositionsName, joints); - ok &= initMetadata(motorStateVelocitiesName, joints); - ok &= initMetadata(motorStateAccelerationsName, joints); - ok &= initMetadata(motorStateCurrentsName, joints); + ok = ok && initMetadata(motorStatePositionsName, joints); + ok = ok && initMetadata(motorStateVelocitiesName, joints); + ok = ok && initMetadata(motorStateAccelerationsName, joints); + ok = ok && initMetadata(motorStateCurrentsName, joints); } if (m_streamMotorPWM) { - ok &= initMetadata(motorStatePwmName, joints); + ok = ok && initMetadata(motorStatePwmName, joints); } if (m_streamPIDs) { - ok &= initMetadata(motorStatePidsName, joints); + ok = ok && initMetadata(motorStatePidsName, joints); } if (m_streamFTSensors) @@ -742,7 +742,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullFTSensorName = ftsName + treeDelim + sensorName; - ok &= initMetadata(fullFTSensorName, ftElementNames); + ok = ok && initMetadata(fullFTSensorName, ftElementNames); } } @@ -752,28 +752,28 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullGyroSensorName = gyrosName + treeDelim + sensorName; - ok &= initMetadata(fullGyroSensorName, gyroElementNames); + ok = ok && initMetadata(fullGyroSensorName, gyroElementNames); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { std::string fullAccelerometerSensorName = accelerometersName + treeDelim + sensorName; - ok &= initMetadata(fullAccelerometerSensorName, AccelerometerElementNames); + ok = ok && initMetadata(fullAccelerometerSensorName, AccelerometerElementNames); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { std::string fullOrientationsSensorName = orientationsName + treeDelim + sensorName; - ok &= initMetadata(fullOrientationsSensorName, orientationElementNames); + ok = ok && initMetadata(fullOrientationsSensorName, orientationElementNames); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { std::string fullMagnetometersSensorName = magnetometersName + treeDelim + sensorName; - ok &= initMetadata(fullMagnetometersSensorName, magnetometerElementNames); + ok = ok && initMetadata(fullMagnetometersSensorName, magnetometerElementNames); } // an IMU contains a gyro accelerometer and an orientation sensor @@ -785,9 +785,9 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) = gyrosName + treeDelim + sensorName; std::string fullOrientationsSensorName = orientationsName + treeDelim + sensorName; - ok &= initMetadata(fullAccelerometerSensorName, AccelerometerElementNames); - ok &= initMetadata(fullGyroSensorName, gyroElementNames); - ok &= initMetadata(fullOrientationsSensorName, orientationElementNames); + ok = ok && initMetadata(fullAccelerometerSensorName, AccelerometerElementNames); + ok = ok && initMetadata(fullGyroSensorName, gyroElementNames); + ok = ok && initMetadata(fullOrientationsSensorName, orientationElementNames); } } @@ -797,7 +797,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullCartesianWrenchName = cartesianWrenchesName + treeDelim + cartesianWrenchName; - ok &= initMetadata(fullCartesianWrenchName, cartesianWrenchNames); + ok = ok && initMetadata(fullCartesianWrenchName, cartesianWrenchNames); } } @@ -807,7 +807,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { std::string fullTemperatureSensorName = temperatureName + treeDelim + sensorName; - ok &= initMetadata(fullTemperatureSensorName, temperatureNames); + ok = ok && initMetadata(fullTemperatureSensorName, temperatureNames); } } From 1c924a9d492964265a4d7e5da0b740bdb5392c95 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 14:46:34 +0100 Subject: [PATCH 78/83] Changed variable names --- .../src/YarpRobotLoggerDevice.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index ff1d2b3b08..b15038778b 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1542,14 +1542,14 @@ void YarpRobotLoggerDevice::run() for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); - BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection + const BipedalLocomotion::YarpUtilities::VectorsCollection* collection = signal.client.readData(false); - if (externalSignalCollection != nullptr) + if (collection != nullptr) { if (!signal.dataArrived) { - for (const auto& [key, vector] : externalSignalCollection->vectors) + for (const auto& [key, vector] : collection->vectors) { signalName = signal.signalName + treeDelim + key; const auto& metadata = signal.metadata.vectors.find(key); @@ -1569,7 +1569,7 @@ void YarpRobotLoggerDevice::run() } else { - for (const auto& [key, vector] : externalSignalCollection->vectors) + for (const auto& [key, vector] : collection->vectors) { signalName = signal.signalName + treeDelim + key; const Eigen::Map> eVector(vector.data(), @@ -1584,8 +1584,8 @@ void YarpRobotLoggerDevice::run() for (auto& [name, signal] : m_vectorSignals) { std::lock_guard lock(signal.mutex); - yarp::sig::Vector* collection = signal.port.read(false); - if (collection != nullptr) + yarp::sig::Vector* vector = signal.port.read(false); + if (vector != nullptr) { if (!signal.dataArrived) { From 9555d00dcfb5c845166c6600713b601ec7991c96 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 14:53:03 +0100 Subject: [PATCH 79/83] reverted to previous signal name --- .../src/YarpRobotLoggerDevice.cpp | 100 +++++++++--------- 1 file changed, 50 insertions(+), 50 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index b15038778b..b7bc7ce449 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -412,7 +412,7 @@ bool YarpRobotLoggerDevice::setupExogenousInputs( for (const auto& input : inputs) { auto group = ptr->getGroup(input).lock(); - std::string signalName, remote; + std::string signalFullName, remote; if (group == nullptr) { log()->error("{} Unable to get the group named {}.", logPrefix, input); @@ -427,7 +427,7 @@ bool YarpRobotLoggerDevice::setupExogenousInputs( return false; } - if (!group->getParameter("signal_name", signalName)) + if (!group->getParameter("signal_name", signalFullName)) { log()->error("{} Unable to get the signal_name parameter for the group named {}.", logPrefix, @@ -435,13 +435,13 @@ bool YarpRobotLoggerDevice::setupExogenousInputs( return false; } - m_vectorsCollectionSignals[remote].signalName = signalName; + m_vectorsCollectionSignals[remote].signalName = signalFullName; if (!m_vectorsCollectionSignals[remote].client.initialize(group)) { log()->error("{} Unable to initialize the vectors collection signal for the group " "named {}.", logPrefix, - signalName); + signalFullName); return false; } } @@ -455,10 +455,10 @@ bool YarpRobotLoggerDevice::setupExogenousInputs( for (const auto& input : inputs) { auto group = ptr->getGroup(input).lock(); - std::string local, signalName, remote, carrier; + std::string local, signalFullName, remote, carrier; if (group == nullptr || !group->getParameter("local", local) || !group->getParameter("remote", remote) || !group->getParameter("carrier", carrier) - || !group->getParameter("signal_name", signalName)) + || !group->getParameter("signal_name", signalFullName)) { log()->error("{} Unable to get the parameters related to the input: {}.", logPrefix, @@ -466,7 +466,7 @@ bool YarpRobotLoggerDevice::setupExogenousInputs( return false; } - m_vectorSignals[remote].signalName = signalName; + m_vectorSignals[remote].signalName = signalFullName; m_vectorSignals[remote].remote = remote; m_vectorSignals[remote].local = local; m_vectorSignals[remote].carrier = carrier; @@ -811,8 +811,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) } } - std::string signalName = ""; - std::string rtSignalName = ""; + std::string signalFullName = ""; + std::string rtSignalFullName = ""; for (auto& [name, signal] : m_vectorsCollectionSignals) { @@ -825,18 +825,18 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& [key, vector] : externalSignalCollection->vectors) { - signalName = signal.signalName + treeDelim + key; + 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, - signalName); - initMetadata(signalName, {}); + signalFullName); + initMetadata(signalFullName, {}); } else { - initMetadata(signalName, {metadata->second}); + initMetadata(signalFullName, {metadata->second}); } } signal.dataArrived = true; @@ -852,7 +852,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { if (!signal.dataArrived) { - initMetadata(signalName, {}); + initMetadata(signalFullName, {}); signal.dataArrived = true; } } @@ -1356,8 +1356,8 @@ void YarpRobotLoggerDevice::run() { constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); - std::string signalName = ""; - std::string rtSignalName = ""; + std::string signalFullName = ""; + std::string rtSignalFullName = ""; std::lock_guard lock(m_bufferManagerMutex); if (m_sendDataRT) @@ -1366,9 +1366,9 @@ void YarpRobotLoggerDevice::run() m_vectorCollectionRTDataServer.clearData(); Eigen::Matrix timeData; timeData << time; - rtSignalName + rtSignalFullName = robotRtRootName + treeDelim + timestampsName; - m_vectorCollectionRTDataServer.populateData(rtSignalName, timeData); + m_vectorCollectionRTDataServer.populateData(rtSignalFullName, timeData); } // get the data @@ -1439,8 +1439,8 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) { - signalName = ftsName + treeDelim + sensorName; - logData(signalName, m_ftBuffer, time); + signalFullName = ftsName + treeDelim + sensorName; + logData(signalFullName, m_ftBuffer, time); } } } @@ -1451,11 +1451,11 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) { - signalName = temperatureName + treeDelim + sensorName; + signalFullName = temperatureName + treeDelim + sensorName; Eigen::Matrix temperatureData; temperatureData << m_ftTemperatureBuffer; - logData(signalName, temperatureData, time); + logData(signalFullName, temperatureData, time); } } } @@ -1466,8 +1466,8 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) { - signalName = gyrosName + treeDelim + sensorName; - logData(signalName, m_gyroBuffer, time); + signalFullName = gyrosName + treeDelim + sensorName; + logData(signalFullName, m_gyroBuffer, time); } } @@ -1477,9 +1477,9 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, m_acceloremeterBuffer)) { - signalName + signalFullName = accelerometersName + treeDelim + sensorName; - logData(signalName, m_acceloremeterBuffer, time); + logData(signalFullName, m_acceloremeterBuffer, time); } } @@ -1489,8 +1489,8 @@ void YarpRobotLoggerDevice::run() if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) { - signalName = orientationsName + treeDelim + sensorName; - logData(signalName, m_orientationBuffer, time); + signalFullName = orientationsName + treeDelim + sensorName; + logData(signalFullName, m_orientationBuffer, time); } } @@ -1498,8 +1498,8 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) { - signalName = magnetometersName + treeDelim + sensorName; - logData(signalName, m_magnemetometerBuffer, time); + signalFullName = magnetometersName + treeDelim + sensorName; + logData(signalFullName, m_magnemetometerBuffer, time); } } } @@ -1515,14 +1515,14 @@ void YarpRobotLoggerDevice::run() m_gyroBuffer, m_orientationBuffer); - signalName = accelerometersName + treeDelim + sensorName; - logData(signalName, m_acceloremeterBuffer, time); + signalFullName = accelerometersName + treeDelim + sensorName; + logData(signalFullName, m_acceloremeterBuffer, time); - signalName = gyrosName + treeDelim + sensorName; - logData(signalName, m_gyroBuffer, time); + signalFullName = gyrosName + treeDelim + sensorName; + logData(signalFullName, m_gyroBuffer, time); - signalName = orientationsName + treeDelim + sensorName; - logData(signalName, m_orientationBuffer, time); + signalFullName = orientationsName + treeDelim + sensorName; + logData(signalFullName, m_orientationBuffer, time); } } @@ -1532,9 +1532,9 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer)) { - signalName = cartesianWrenchesName + treeDelim + signalFullName = cartesianWrenchesName + treeDelim + cartesianWrenchName; - logData(signalName, m_ftBuffer, time); + logData(signalFullName, m_ftBuffer, time); } } } @@ -1551,18 +1551,18 @@ void YarpRobotLoggerDevice::run() { for (const auto& [key, vector] : collection->vectors) { - signalName = signal.signalName + treeDelim + key; + 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, - signalName); - initMetadata(signalName, {}); + signalFullName); + initMetadata(signalFullName, {}); } else { - initMetadata(signalName, {metadata->second}); + initMetadata(signalFullName, {metadata->second}); } } signal.dataArrived = true; @@ -1571,10 +1571,10 @@ void YarpRobotLoggerDevice::run() { for (const auto& [key, vector] : collection->vectors) { - signalName = signal.signalName + treeDelim + key; + signalFullName = signal.signalName + treeDelim + key; const Eigen::Map> eVector(vector.data(), vector.size()); - logData(signalName, eVector, time); + logData(signalFullName, eVector, time); } } @@ -1589,7 +1589,7 @@ void YarpRobotLoggerDevice::run() { if (!signal.dataArrived) { - initMetadata(signalName, {}); + initMetadata(signalFullName, {}); signal.dataArrived = true; } } @@ -1609,18 +1609,18 @@ void YarpRobotLoggerDevice::run() msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, std::to_string(time)); if (msg.isValid) { - signalName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName + "::p" + signalFullName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName + "::p" + msg.processPID; // matlab does not support the character - as a key of a struct - findAndReplaceAll(signalName, "-", "_"); + findAndReplaceAll(signalFullName, "-", "_"); // if it is the first time this signal is seen by the device the channel is // added - if (m_textLogsStoredInManager.find(signalName) == m_textLogsStoredInManager.end()) + if (m_textLogsStoredInManager.find(signalFullName) == m_textLogsStoredInManager.end()) { - m_bufferManager.addChannel({signalName, {1, 1}}); - m_textLogsStoredInManager.insert(signalName); + m_bufferManager.addChannel({signalFullName, {1, 1}}); + m_textLogsStoredInManager.insert(signalFullName); } } bufferportSize = m_textLoggingPort.getPendingReads(); From a91ee014d250b9598fb6357474db282cba5b3dc2 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 15:07:54 +0100 Subject: [PATCH 80/83] Cleaned up python bindings --- .../YarpUtilities/src/VectorsCollection.cpp | 21 +------------------ 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index 7ef2985e0f..b5e1a67543 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -88,27 +88,8 @@ void CreateVectorsCollectionMetadata(pybind11::module& module) py::class_(module, "VectorsCollectionMetadata") .def(py::init()) .def(py::init>&>()) - .def("readWireReader", - [](VectorsCollectionMetadata& impl, yarp::os::idl::WireReader& reader) -> bool { - return impl.read(reader); - }) - .def("readConnectionReader", - [](VectorsCollectionMetadata& impl, yarp::os::ConnectionReader& connection) -> bool { - return impl.read(connection); - }) - .def("writeWireWriter", - [](const VectorsCollectionMetadata& impl, const yarp::os::idl::WireWriter& writer) -> bool { - return impl.write(writer); - }) - .def("writeConnectionWriter", - [](const VectorsCollectionMetadata& impl, yarp::os::ConnectionWriter& connection) -> bool { - return impl.write(connection); - }) .def("to_string", &VectorsCollectionMetadata::toString) - .def("getVectors", - [](const VectorsCollectionMetadata& impl) -> std::map> { - return impl.vectors; - }); + .def_readwrite("vectors", &VectorsCollectionMetadata::vectors); } } // namespace YarpUtilities } // namespace bindings From 0888c9631b6c934ab69fce248eae0c9587910fc7 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 16:09:41 +0100 Subject: [PATCH 81/83] Turned log data into a lambda --- .../BipedalLocomotion/YarpRobotLoggerDevice.h | 3 --- .../src/YarpRobotLoggerDevice.cpp | 22 +++++++++---------- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 4e9ae6a5a4..6e257740b6 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -178,9 +178,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void lookForExogenousSignals(); bool initMetadata(const std::string& nameKey, const std::vector& metadata); - void logData(const std::string& name, - const Eigen::VectorXd& data, - const double time); bool hasSubstring(const std::string& str, const std::vector& substrings) const; void recordVideo(const std::string& cameraName, VideoWriter& writer); diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index b7bc7ce449..37e5c955a4 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -639,17 +639,6 @@ bool YarpRobotLoggerDevice::initMetadata(const std::string& nameKey, const std:: return ok; } -void YarpRobotLoggerDevice::logData(const std::string& name, - const Eigen::VectorXd& data, - const double time) -{ - m_bufferManager.push_back(data, time, name); - std::string rtName = robotRtRootName + treeDelim + name; - if (m_sendDataRT) - { - m_vectorCollectionRTDataServer.populateData(rtName, data); - } -} bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { @@ -1354,6 +1343,17 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit void YarpRobotLoggerDevice::run() { + auto logData = [this](const std::string& name, + const auto& data, + const double time) { + m_bufferManager.push_back(data, time, name); + std::string rtName = robotRtRootName + treeDelim + name; + if (m_sendDataRT) + { + m_vectorCollectionRTDataServer.populateData(rtName, data); + } + }; + constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]"; const double time = std::chrono::duration(BipedalLocomotion::clock().now()).count(); std::string signalFullName = ""; From 01aea4eb552e9f1d2278e023be738f266b266330 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 6 Mar 2024 16:24:36 +0100 Subject: [PATCH 82/83] Removed Eigen Map after logData lambda change --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 37e5c955a4..ded7e5a071 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -1572,9 +1572,7 @@ void YarpRobotLoggerDevice::run() for (const auto& [key, vector] : collection->vectors) { signalFullName = signal.signalName + treeDelim + key; - const Eigen::Map> eVector(vector.data(), - vector.size()); - logData(signalFullName, eVector, time); + logData(signalFullName, vector, time); } } From 0c28b90cf5d173f8dad79c7daf3ea95ea4bb9d81 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Mon, 11 Mar 2024 09:53:59 +0100 Subject: [PATCH 83/83] removed duplicate include of vector Co-authored-by: Stefano Dafarra --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 1 - 1 file changed, 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 6e257740b6..a839b157ae 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -13,7 +13,6 @@ #include #include #include -#include #include #include