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 diff --git a/CHANGELOG.md b/CHANGELOG.md index 09193e1931..60f73025e1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,6 +3,7 @@ All notable changes to this project are documented in this file. ## [unreleased] ### Added +- Added Vector Collection Server for publishing information for real-time users in the YARPRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/796) - Set submodel states from IMUs in RDE and add friction torques as measurement (https://github.com/ami-iit/bipedal-locomotion-framework/pull/793) - Add streaming of arm fts in YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/803) @@ -32,6 +33,7 @@ All notable changes to this project are documented in this file. - Implement `blf-joints-grid-position-tracking` application in `utilities` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/787) - Add the possibility to resample the contact in a given contact list (https://github.com/ami-iit/bipedal-locomotion-framework/pull/788) + ### 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) - Restructured the swing foot planner to handle corners case that came out while testing DNN-MPC integration (https://github.com/ami-iit/bipedal-locomotion-framework/pull/765) diff --git a/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h b/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h index 1c54f5ce2a..e856c950da 100644 --- a/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h +++ b/bindings/python/YarpUtilities/include/BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h @@ -18,6 +18,8 @@ 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 660f20cb63..05f310d6db 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); + CreateVectorsCollectionMetadata(module); } } // namespace IK } // namespace bindings diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index 848b6b0019..b5e1a67543 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -10,6 +10,7 @@ #include #include +#include #include #include @@ -46,6 +47,50 @@ void CreateVectorsCollectionServer(pybind11::module& module) }) .def("prepare_data", &VectorsCollectionServer::prepareData); } + +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("get_metadata", + [](VectorsCollectionClient& impl) -> BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata + { + BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata metadata; + impl.getMetadata(metadata); + return metadata; + }) + .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("to_string", &VectorsCollectionMetadata::toString) + .def_readwrite("vectors", &VectorsCollectionMetadata::vectors); +} } // namespace YarpUtilities } // namespace bindings } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 5552e57de9..a839b157ae 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -9,8 +9,8 @@ #include #include #include +#include #include -#include #include #include @@ -30,6 +30,7 @@ #include #include #include +#include namespace BipedalLocomotion { @@ -62,6 +63,9 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::unique_ptr m_robotSensorBridge; std::unique_ptr m_cameraBridge; + bool m_sendDataRT; + BipedalLocomotion::YarpUtilities::VectorsCollectionServer m_vectorCollectionRTDataServer; + template struct ExogenousSignal { std::mutex mutex; @@ -98,7 +102,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool connect(); void disconnect(); - }; std::unordered_map m_vectorsCollectionSignals; @@ -167,10 +170,14 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::vector m_codeStatusCmdPrefixes; std::mutex m_bufferManagerMutex; + std::mutex m_textLoggingPortMutex; robometry::BufferManager m_bufferManager; void lookForNewLogs(); void lookForExogenousSignals(); + + bool initMetadata(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, @@ -191,8 +198,60 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool createFramesFolder(std::shared_ptr imageSaver, const std::string& camera, const std::string& imageType); + + const std::string treeDelim = "::"; + + const std::string robotRtRootName = "robot_realtime"; + + 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 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 motorStatePidsName = "PIDs"; + + const std::string ftsName = "FTs"; + + const std::vector ftElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}; + + const std::string gyrosName = "gyros"; + const std::vector gyroElementNames = {"omega_x", "omega_y", "omega_z"}; + + const std::string accelerometersName = "accelerometers"; + const std::vector AccelerometerElementNames = {"a_x", "a_y", "a_z"}; + + const std::string orientationsName = "orientations"; + const std::vector orientationElementNames = {"r", "p", "y"}; + + const std::string magnetometersName = "magnetometers"; + const std::vector magnetometerElementNames = {"mag_x", "mag_y", "mag_z"}; + + 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 temperatureName = "temperatures"; + const std::vector temperatureNames = {"temperature"}; + + const std::string robotName = "yarp_robot_name"; + + const std::string robotDescriptionList = "description_list"; + + const std::string timestampsName = "timestamps"; + }; } // namespace BipedalLocomotion + #endif // BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_H 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 3725d31e49..ded7e5a071 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include @@ -29,6 +28,8 @@ #include #include +#include + #include #include #include @@ -103,6 +104,8 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period, // the logging message are streamed using yarp BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); + + m_sendDataRT = false; } YarpRobotLoggerDevice::YarpRobotLoggerDevice() @@ -115,14 +118,34 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() // the logging message are streamed using yarp BipedalLocomotion::TextLogging::LoggerBuilder::setFactory( std::make_shared()); + + m_sendDataRT = false; } YarpRobotLoggerDevice::~YarpRobotLoggerDevice() = default; bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) { + constexpr auto logPrefix = "[YarpRobotLoggerDevice::open]"; auto params = std::make_shared(config); + auto rtParameters = params->getGroup("REAL_TIME_STREAMING").lock(); + m_sendDataRT = rtParameters != nullptr; + if (m_sendDataRT) + { + 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 + { + log()->info("{} Real time logging not activated", logPrefix); + } double devicePeriod{0.01}; if (params->getParameter("sampling_period_in_s", devicePeriod)) @@ -389,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); @@ -404,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, @@ -412,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; } } @@ -432,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, @@ -443,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; @@ -605,10 +628,31 @@ bool YarpRobotLoggerDevice::setupRobotCameraBridge( return true; } +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; + if (m_sendDataRT) + { + ok = ok && m_vectorCollectionRTDataServer.populateMetadata(rtNameKey, metadataNames); + } + return ok; +} + + 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,43 +683,55 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) const unsigned dofs = joints.size(); m_bufferManager.setDescriptionList(joints); + if (m_sendDataRT) + { + char* tmp = std::getenv("YARP_ROBOT_NAME"); + std::string metadataName + = robotRtRootName + treeDelim + robotName; + m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)}); - bool ok = true; + metadataName + = robotRtRootName + treeDelim + timestampsName; + m_vectorCollectionRTDataServer.populateMetadata(metadataName, {timestampsName}); + + std::string rtMetadataName = robotRtRootName + treeDelim + + robotDescriptionList; + m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints); + } // 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 && initMetadata(jointStatePositionsName, joints); + ok = ok && initMetadata(jointStateVelocitiesName, joints); + ok = ok && initMetadata(jointStateAccelerationsName, joints); + ok = ok && initMetadata(jointStateTorquesName, 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 && initMetadata(motorStatePositionsName, joints); + ok = ok && initMetadata(motorStateVelocitiesName, joints); + ok = ok && initMetadata(motorStateAccelerationsName, joints); + ok = ok && initMetadata(motorStateCurrentsName, joints); } if (m_streamMotorPWM) { - ok = ok && m_bufferManager.addChannel({"motors_state::PWM", {dofs, 1}, joints}); + ok = ok && initMetadata(motorStatePwmName, joints); } if (m_streamPIDs) { - ok = ok && m_bufferManager.addChannel({"PIDs", {dofs, 1}, joints}); + ok = ok && initMetadata(motorStatePidsName, joints); } if (m_streamFTSensors) { for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { - ok = ok - && m_bufferManager.addChannel({"FTs::" + sensorName, - {6, 1}, // - {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}}); + std::string fullFTSensorName + = ftsName + treeDelim + sensorName; + ok = ok && initMetadata(fullFTSensorName, ftElementNames); } } @@ -683,60 +739,54 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { - ok = ok - && m_bufferManager.addChannel({"gyros::" + sensorName, - {3, 1}, // - {"omega_x", "omega_y", "omega_z"}}); + std::string fullGyroSensorName + = gyrosName + treeDelim + sensorName; + ok = ok && initMetadata(fullGyroSensorName, gyroElementNames); } for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { - ok = ok - && m_bufferManager.addChannel({"accelerometers::" + sensorName, - {3, 1}, // - {"a_x", "a_y", "a_z"}}); + std::string fullAccelerometerSensorName + = accelerometersName + treeDelim + sensorName; + ok = ok && initMetadata(fullAccelerometerSensorName, AccelerometerElementNames); } for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { - ok = ok - && m_bufferManager.addChannel({"orientations::" + sensorName, - {3, 1}, // - {"r", "p", "y"}}); + std::string fullOrientationsSensorName + = orientationsName + treeDelim + sensorName; + ok = ok && initMetadata(fullOrientationsSensorName, orientationElementNames); } for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { - ok = ok - && m_bufferManager.addChannel({"magnetometers::" + sensorName, - {3, 1}, // - {"mag_x", "mag_y", "mag_z"}}); + std::string fullMagnetometersSensorName + = magnetometersName + treeDelim + sensorName; + ok = ok && initMetadata(fullMagnetometersSensorName, magnetometerElementNames); } // an IMU contains a gyro accelerometer and an orientation sensor for (const auto& sensorName : m_robotSensorBridge->getIMUsList()) { - ok = ok - && m_bufferManager.addChannel({"accelerometers::" + sensorName, - {3, 1}, // - {"a_x", "a_y", "a_z"}}) - && m_bufferManager.addChannel({"gyros::" + sensorName, - {3, 1}, // - {"omega_x", "omega_y", "omega_z"}}) - && m_bufferManager.addChannel({"orientations::" + sensorName, - {3, 1}, // - {"r", "p", "y"}}); + std::string fullAccelerometerSensorName + = accelerometersName + treeDelim + sensorName; + std::string fullGyroSensorName + = gyrosName + treeDelim + sensorName; + std::string fullOrientationsSensorName + = orientationsName + treeDelim + sensorName; + ok = ok && initMetadata(fullAccelerometerSensorName, AccelerometerElementNames); + ok = ok && initMetadata(fullGyroSensorName, gyroElementNames); + ok = ok && initMetadata(fullOrientationsSensorName, orientationElementNames); } } if (m_streamCartesianWrenches) { - for (const auto& sensorName : m_robotSensorBridge->getCartesianWrenchesList()) + for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList()) { - ok = ok - && m_bufferManager.addChannel({"cartesian_wrenches::" + sensorName, - {6, 1}, // - {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"}}); + std::string fullCartesianWrenchName = cartesianWrenchesName + + treeDelim + cartesianWrenchName; + ok = ok && initMetadata(fullCartesianWrenchName, cartesianWrenchNames); } } @@ -744,23 +794,66 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) { for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { - ok = ok - && m_bufferManager.addChannel({"temperatures::" + sensorName, - {1, 1}, // - {"temperature"}}); + std::string fullTemperatureSensorName + = temperatureName + treeDelim + sensorName; + ok = ok && initMetadata(fullTemperatureSensorName, temperatureNames); } } - // resize the temporary vectors - m_jointSensorBuffer.resize(dofs); + std::string signalFullName = ""; + std::string rtSignalFullName = ""; - // open the TextLogging port - ok = ok && m_textLoggingPort.open(m_textLoggingPortName); - // run the thread - m_lookForNewLogsThread = std::thread([this] { this->lookForNewLogs(); }); + for (auto& [name, signal] : m_vectorsCollectionSignals) + { + std::lock_guard lock(signal.mutex); + BipedalLocomotion::YarpUtilities::VectorsCollection* externalSignalCollection + = signal.client.readData(false); + if (externalSignalCollection != nullptr) + { + if (!signal.dataArrived) + { + for (const auto& [key, vector] : externalSignalCollection->vectors) + { + signalFullName = signal.signalName + treeDelim + key; + const auto& metadata = signal.metadata.vectors.find(key); + if (metadata == signal.metadata.vectors.cend()) + { + log()->warn("{} Unable to find the metadata for the signal named {}. The " + "default one will be used.", + logPrefix, + signalFullName); + initMetadata(signalFullName, {}); + } else + { + initMetadata(signalFullName, {metadata->second}); + } + } + signal.dataArrived = true; + } + } + } - // run the thread for reading the exogenous signals - m_lookForNewExogenousSignalThread = std::thread([this] { this->lookForExogenousSignals(); }); + 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) + { + initMetadata(signalFullName, {}); + signal.dataArrived = true; + } + } + } + + if (m_sendDataRT) + { + m_vectorCollectionRTDataServer.finalizeMetadata(); + } + + // resize the temporary vectors + m_jointSensorBuffer.resize(dofs); // The user can avoid to record the camera if (m_cameraBridge != nullptr) @@ -1005,8 +1098,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) { @@ -1092,20 +1186,25 @@ void YarpRobotLoggerDevice::lookForNewLogs() // check for new messages yarp::profiler::NetworkProfiler::getPortsList(yarpPorts); - 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"); + } } - } + } // end of scope for lock guarding text logging port // release the CPU BipedalLocomotion::clock().yield(); @@ -1244,7 +1343,33 @@ 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 = ""; + std::string rtSignalFullName = ""; + + std::lock_guard lock(m_bufferManagerMutex); + if (m_sendDataRT) + { + m_vectorCollectionRTDataServer.prepareData(); + m_vectorCollectionRTDataServer.clearData(); + Eigen::Matrix timeData; + timeData << time; + rtSignalFullName + = robotRtRootName + treeDelim + timestampsName; + m_vectorCollectionRTDataServer.populateData(rtSignalFullName, timeData); + } // get the data if (!m_robotSensorBridge->advance()) @@ -1252,27 +1377,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); - // collect the data if (m_streamJointStates) { if (m_robotSensorBridge->getJointPositions(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::positions"); + logData(jointStatePositionsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointVelocities(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::velocities"); + logData(jointStateVelocitiesName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointAccelerations(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::accelerations"); + logData(jointStateAccelerationsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getJointTorques(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "joints_state::torques"); + logData(jointStateTorquesName, m_jointSensorBuffer, time); } } @@ -1280,19 +1401,19 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPositions(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::positions"); + logData(motorStatePositionsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorVelocities(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::velocities"); + logData(motorStateVelocitiesName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorAccelerations(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::accelerations"); + logData(motorStateAccelerationsName, m_jointSensorBuffer, time); } if (m_robotSensorBridge->getMotorCurrents(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::currents"); + logData(motorStateCurrentsName, m_jointSensorBuffer, time); } } @@ -1300,7 +1421,7 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getMotorPWMs(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "motors_state::PWM"); + logData(motorStatePwmName, m_jointSensorBuffer, time); } } @@ -1308,56 +1429,78 @@ void YarpRobotLoggerDevice::run() { if (m_robotSensorBridge->getPidPositions(m_jointSensorBuffer)) { - m_bufferManager.push_back(m_jointSensorBuffer, time, "PIDs"); + logData(motorStatePidsName, m_jointSensorBuffer, time); } } - for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) + if (m_streamFTSensors) { - if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) + for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList()) { - m_bufferManager.push_back(m_ftBuffer, time, "FTs::" + sensorName); + if (m_robotSensorBridge->getSixAxisForceTorqueMeasurement(sensorName, m_ftBuffer)) + { + signalFullName = ftsName + treeDelim + sensorName; + logData(signalFullName, m_ftBuffer, time); + } } } - for (const auto& sensorname : m_robotSensorBridge->getTemperatureSensorsList()) + if (m_streamTemperatureSensors) { - if (m_robotSensorBridge->getTemperature(sensorname, m_ftTemperatureBuffer)) + for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList()) { - m_bufferManager.push_back({m_ftTemperatureBuffer}, time, "temperatures::" + sensorname); + if (m_robotSensorBridge->getTemperature(sensorName, m_ftTemperatureBuffer)) + { + signalFullName = temperatureName + treeDelim + sensorName; + + Eigen::Matrix temperatureData; + temperatureData << m_ftTemperatureBuffer; + logData(signalFullName, temperatureData, time); + } } } - for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) + if (m_streamInertials) { - if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) + for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList()) { - m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); + if (m_robotSensorBridge->getGyroscopeMeasure(sensorName, m_gyroBuffer)) + { + signalFullName = gyrosName + treeDelim + sensorName; + logData(signalFullName, m_gyroBuffer, time); + } } - } - for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) - { - if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, - m_acceloremeterBuffer)) + // pack the data for the accelerometer + for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList()) { - m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); + if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName, + m_acceloremeterBuffer)) + { + signalFullName + = accelerometersName + treeDelim + sensorName; + logData(signalFullName, m_acceloremeterBuffer, time); + } } - } - for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) - { - if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, m_orientationBuffer)) + // pack the data for the orientations + for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList()) { - m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); + if (m_robotSensorBridge->getOrientationSensorMeasurement(sensorName, + m_orientationBuffer)) + { + signalFullName = orientationsName + treeDelim + sensorName; + logData(signalFullName, m_orientationBuffer, time); + } } - } - for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) - { - if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) + for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList()) { - m_bufferManager.push_back(m_magnemetometerBuffer, time, "magnetometers::" + sensorName); + if (m_robotSensorBridge->getMagnetometerMeasurement(sensorName, m_magnemetometerBuffer)) + { + signalFullName = magnetometersName + treeDelim + sensorName; + logData(signalFullName, m_magnemetometerBuffer, time); + } } } @@ -1366,39 +1509,49 @@ 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, m_orientationBuffer); - m_bufferManager.push_back(m_acceloremeterBuffer, time, "accelerometers::" + sensorName); - m_bufferManager.push_back(m_gyroBuffer, time, "gyros::" + sensorName); - m_bufferManager.push_back(m_orientationBuffer, time, "orientations::" + sensorName); + signalFullName = accelerometersName + treeDelim + sensorName; + logData(signalFullName, m_acceloremeterBuffer, time); + + signalFullName = gyrosName + treeDelim + sensorName; + logData(signalFullName, m_gyroBuffer, time); + + signalFullName = orientationsName + treeDelim + sensorName; + logData(signalFullName, m_orientationBuffer, time); } } - 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)) + { + signalFullName = cartesianWrenchesName + treeDelim + + cartesianWrenchName; + logData(signalFullName, m_ftBuffer, time); + } } } - std::string signalFullName; for (auto& [name, signal] : m_vectorsCollectionSignals) { std::lock_guard lock(signal.mutex); const BipedalLocomotion::YarpUtilities::VectorsCollection* collection = signal.client.readData(false); + if (collection != nullptr) { if (!signal.dataArrived) { for (const auto& [key, vector] : collection->vectors) { - signalFullName = signal.signalName + "::" + key; + signalFullName = signal.signalName + treeDelim + key; const auto& metadata = signal.metadata.vectors.find(key); if (metadata == signal.metadata.vectors.cend()) { @@ -1406,23 +1559,22 @@ void YarpRobotLoggerDevice::run() "default one will be used.", logPrefix, signalFullName); - m_bufferManager.addChannel({signalFullName, {vector.size(), 1}}); + initMetadata(signalFullName, {}); } else { - // if the metadata is found we use it - m_bufferManager.addChannel({signalFullName, // - {vector.size(), 1}, - metadata->second}); + initMetadata(signalFullName, {metadata->second}); } - - signal.dataArrived = true; } + signal.dataArrived = true; } - - for (const auto& [key, vector] : collection->vectors) + else { - signalFullName = signal.signalName + "::" + key; - m_bufferManager.push_back(vector, time, signalFullName); + for (const auto& [key, vector] : collection->vectors) + { + signalFullName = signal.signalName + treeDelim + key; + logData(signalFullName, vector, time); + } + } } } @@ -1435,46 +1587,51 @@ void YarpRobotLoggerDevice::run() { if (!signal.dataArrived) { - m_bufferManager.addChannel({signal.signalName, {vector->size(), 1}}); + initMetadata(signalFullName, {}); signal.dataArrived = true; } - m_bufferManager.push_back(*vector, time, signal.signalName); } } - 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) { - signalFullName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName - + "::p" + msg.processPID; + msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, std::to_string(time)); + if (msg.isValid) + { + signalFullName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName + "::p" + + msg.processPID; - // matlab does not support the character - as a key of a struct - findAndReplaceAll(signalFullName, "-", "_"); + // matlab does not support the character - as a key of a struct + findAndReplaceAll(signalFullName, "-", "_"); - // if it is the first time this signal is seen by the device the channel is - // added - if (m_textLogsStoredInManager.find(signalFullName) - == m_textLogsStoredInManager.end()) - { - m_bufferManager.addChannel({signalFullName, {1, 1}}); - m_textLogsStoredInManager.insert(signalFullName); + // if it is the first time this signal is seen by the device the channel is + // added + if (m_textLogsStoredInManager.find(signalFullName) == m_textLogsStoredInManager.end()) + { + m_bufferManager.addChannel({signalFullName, {1, 1}}); + m_textLogsStoredInManager.insert(signalFullName); + } } - - m_bufferManager.push_back(msg, time, signalFullName); + bufferportSize = m_textLoggingPort.getPendingReads(); + } else + { + break; } - bufferportSize = m_textLoggingPort.getPendingReads(); - } else - { - break; } + } // end of lock guard scope for text logging port + + if (m_sendDataRT) + { + m_vectorCollectionRTDataServer.sendData(); } }