RT Capabilities Part 1 - YARP Logger Device

.gitignore

@@ -155,3 +155,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)
 
@@ -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)

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,69 @@ 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("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<std::string, std::vector<std::string>> {
+ return impl.vectors;
+ });
+}
 } // namespace YarpUtilities
 } // namespace bindings
 } // namespace BipedalLocomotion

devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h

@@ -9,10 +9,11 @@
 #include <atomic>
 #include <memory>
 #include <mutex>
+#include <vector>
 #include <string>
-#include <string_view>
 #include <unordered_map>
 #include <unordered_set>
+#include <vector>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/videoio.hpp>
@@ -30,6 +31,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 +64,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 +103,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
 
  bool connect();
  void disconnect();
-
  };
 
  std::unordered_map<std::string, VectorsCollectionSignal> m_vectorsCollectionSignals;
@@ -167,10 +171,17 @@ 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);
+ void logData(const std::string& name,
+ const Eigen::VectorXd& data,
+ const double time);
+
  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 +202,59 @@ 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