Added Vector Collection Server for publishing information for real-ti…

…me users in the YARPRobotLoggerDevice (#817)

.gitignore

@@ -157,3 +157,6 @@ dmypy.json
 
 # Cython debug symbols
 cython_debug/
+
+# compile commands file for resolving includes
+compile_commands.json

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)
 - 🤖  [ `ergoCubGazeboV1_1`] Add configuration files to log data with the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/806, https://github.com/ami-iit/bipedal-locomotion-framework/pull/808)
@@ -42,6 +43,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)

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

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

bindings/python/YarpUtilities/src/VectorsCollection.cpp

@@ -10,6 +10,7 @@
 #include <pybind11/eigen.h>
 
 #include <BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h>
+#include <BipedalLocomotion/YarpUtilities/VectorsCollectionClient.h>
 
 #include <BipedalLocomotion/bindings/YarpUtilities/BufferedPort.h>
 #include <BipedalLocomotion/bindings/YarpUtilities/VectorsCollection.h>
@@ -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_<VectorsCollectionClient>(module, "VectorsCollectionClient")
+        .def(py::init())
+        .def(
+            "initialize",
+            [](VectorsCollectionClient& impl,
+               std::shared_ptr<const ::BipedalLocomotion::ParametersHandler::IParametersHandler>
+                   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<std::string, std::vector<double>>
+             {
+                BipedalLocomotion::YarpUtilities::VectorsCollection* collection = impl.readData(shouldWait);
+                return collection->vectors;
+             });
+}
+
+void CreateVectorsCollectionMetadata(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+
+    using namespace ::BipedalLocomotion::YarpUtilities;
+
+    py::class_<VectorsCollectionMetadata>(module, "VectorsCollectionMetadata")
+        .def(py::init())
+        .def(py::init<const std::map<std::string, std::vector<std::string>>&>())
+        .def("to_string", &VectorsCollectionMetadata::toString)
+        .def_readwrite("vectors", &VectorsCollectionMetadata::vectors);
+}
 } // namespace YarpUtilities
 } // namespace bindings
 } // namespace BipedalLocomotion

devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h

@@ -9,8 +9,8 @@
 #include <atomic>
 #include <memory>
 #include <mutex>
+#include <vector>
 #include <string>
-#include <string_view>
 #include <unordered_map>
 #include <unordered_set>
 
@@ -30,6 +30,7 @@
 #include <BipedalLocomotion/RobotInterface/YarpSensorBridge.h>
 #include <BipedalLocomotion/YarpUtilities/VectorsCollection.h>
 #include <BipedalLocomotion/YarpUtilities/VectorsCollectionClient.h>
+#include <BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h>
 
 namespace BipedalLocomotion
 {
@@ -62,6 +63,9 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
     std::unique_ptr<BipedalLocomotion::RobotInterface::YarpSensorBridge> m_robotSensorBridge;
     std::unique_ptr<BipedalLocomotion::RobotInterface::YarpCameraBridge> m_cameraBridge;
 
+    bool m_sendDataRT;
+    BipedalLocomotion::YarpUtilities::VectorsCollectionServer m_vectorCollectionRTDataServer;
+
     template <typename T> struct ExogenousSignal
     {
         std::mutex mutex;
@@ -98,7 +102,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
 
         bool connect();
         void disconnect();
-
     };
 
     std::unordered_map<std::string, VectorsCollectionSignal> m_vectorsCollectionSignals;
@@ -167,10 +170,14 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
     std::vector<std::string> 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<std::string>& metadata);
+
     bool hasSubstring(const std::string& str, const std::vector<std::string>& substrings) const;
     void recordVideo(const std::string& cameraName, VideoWriter& writer);
     void unpackIMU(Eigen::Ref<const analog_sensor_t> signal,
@@ -191,8 +198,60 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
     bool createFramesFolder(std::shared_ptr<VideoWriter::ImageSaver> 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<std::string> ftElementNames = {"f_x", "f_y", "f_z", "mu_x", "mu_y", "mu_z"};
+
+    const std::string gyrosName = "gyros";
+    const std::vector<std::string> gyroElementNames = {"omega_x", "omega_y", "omega_z"};
+
+    const std::string accelerometersName = "accelerometers";
+    const std::vector<std::string> AccelerometerElementNames = {"a_x", "a_y", "a_z"};
+
+    const std::string orientationsName = "orientations";
+    const std::vector<std::string> orientationElementNames = {"r", "p", "y"};
+
+    const std::string magnetometersName = "magnetometers";
+    const std::vector<std::string> magnetometerElementNames = {"mag_x", "mag_y", "mag_z"};
+
+    const std::string cartesianWrenchesName = "cartesian_wrenches";
+    const std::vector<std::string> cartesianWrenchNames = {ftElementNames[0],
+                                                        ftElementNames[1],
+                                                        ftElementNames[2],
+                                                        ftElementNames[3],
+                                                        ftElementNames[4],
+                                                        ftElementNames[5]};
+
+    const std::string temperatureName = "temperatures";
+    const std::vector<std::string> 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

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 <string>
 #include <cstdint>
+#include <string>
 
 #include <yarp/os/Bottle.h>
 
@@ -57,5 +57,4 @@ struct TextLoggingEntry
 
 } // namespace BipedalLocomotion
 
-
 #endif // BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_YARP_TEXT_LOGGING_UTILITIES_H