robotology · Nicogene · Nov 7, 2024 · Sep 4, 2024 · Oct 22, 2024 · Nov 1, 2024
diff --git a/README.md b/README.md
@@ -479,7 +479,7 @@ The configuration can be saved **to a json file**
 
 ## TelemetryDeviceDumper
 
-The `telemetryDeviceDumper` is a [yarp device](http://yarp.it/git-master/note_devices.html) that has to be launched through the [`yarprobotinterface`](http://yarp.it/git-master/yarprobotinterface.html) for dumping quantities from your robot(e.g. encoders, velocities etc.) in base of what specified in the configuration.
+The `telemetryDeviceDumper` is a [yarp device](http://yarp.it/git-master/note_devices.html) that has to be launched through the [`yarprobotinterface`](http://yarp.it/git-master/yarprobotinterface.html) for dumping quantities from your robot(e.g. encoders, velocities etc.) in base of what specified in the configuration. It currently needs icub-main version equal or higher than `2.7.0` Specificially this is needed when enabling the parameter `logIRawValuesPublisher`, which is used for dumping any type of raw data values coming from the low level, e.g. raw encoder data. 
 
 ### Export the env variables
 * Add `${CMAKE_INSTALL_PREFIX}/share/yarp` (where `${CMAKE_INSTALL_PREFIX}` needs to be substituted to the directory that you choose as the `CMAKE_INSTALL_PREFIX`) to your `YARP_DATA_DIRS` environment variable (for more on the `YARP_DATA_DIRS` env variable, see [YARP documentation on data directories](http://www.yarp.it/yarp_data_dirs.html) ).

diff --git a/src/telemetryDeviceDumper/CMakeLists.txt b/src/telemetryDeviceDumper/CMakeLists.txt
@@ -20,12 +20,14 @@ if(ENABLE_telemetryDeviceDumper)
     add_definitions(-D_USE_MATH_DEFINES)
   endif()
 
+  find_package(iCubDev 2.7.0 REQUIRED)
   target_sources(yarp_telemetryDeviceDumper PRIVATE TelemetryDeviceDumper.cpp
                                                     TelemetryDeviceDumper.h)
-
+                        
   target_link_libraries(yarp_telemetryDeviceDumper PRIVATE YARP::YARP_os
                                                            YARP::YARP_dev
-                                                           robometry::robometry)
+                                                           robometry::robometry
+                                                           ICUB::iCubDev)
   list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os
                                                       YARP_dev
                                                       robometry)

diff --git a/src/telemetryDeviceDumper/TelemetryDeviceDumper.cpp b/src/telemetryDeviceDumper/TelemetryDeviceDumper.cpp
@@ -13,6 +13,7 @@
 using namespace robometry;
 using namespace yarp::os;
 using namespace yarp::dev;
+using namespace iCub;
 
 constexpr double log_thread_default{ 0.010 };
 
@@ -117,6 +118,12 @@ bool TelemetryDeviceDumper::loadSettingsFromConfig(yarp::os::Searchable& config)
         settings.logILocalization2D = prop.find(logILocalization2DOptionName.c_str()).asBool();
     }
 
+    std::string logIRawValuesPublisherOptionName = "logIRawValuesPublisher";
+    if (prop.check(logIRawValuesPublisherOptionName.c_str())) 
+    {
+        settings.logIRawValuesPublisher = prop.find(logIRawValuesPublisherOptionName.c_str()).asBool();
+    }
+
     std::string useRadians = "useRadians";
     if (prop.check(useRadians.c_str())) {
         settings.useRadians = prop.find(useRadians.c_str()).asBool();
@@ -132,6 +139,11 @@ bool TelemetryDeviceDumper::loadSettingsFromConfig(yarp::os::Searchable& config)
         settings.localizationRemoteName = prop.find(localizationRemoteName.c_str()).asString();
     }
 
+    std::string rawValuesPublisherRemoteName = "rawValuesPublisherRemoteName";
+    if (prop.check(rawValuesPublisherRemoteName.c_str()) && prop.find(rawValuesPublisherRemoteName.c_str()).isString()) {
+        settings.rawValuesPublisherRemoteName = prop.find(rawValuesPublisherRemoteName.c_str()).asString();
+    }
+
     // BufferManager options
     std::string json_file = "json_file";
     if (prop.check(json_file.c_str()) && prop.find(json_file.c_str()).isString()) {
@@ -229,7 +241,7 @@ bool TelemetryDeviceDumper::open(yarp::os::Searchable& config) {
     if (settings.logILocalization2D) {
         yarp::os::Property loc2DClientProp{{"device", Value("localization2DClient")},
                                            {"remote", Value(settings.localizationRemoteName)},
-                                           {"local",  Value("/telemetryDeviceDumper" + settings.localizationRemoteName + "/client")}};;
+                                           {"local",  Value("/telemetryDeviceDumper" + settings.localizationRemoteName + "/client")}};
         ok = this->localization2DClient.open(loc2DClientProp);
         ok = ok && this->localization2DClient.view(iloc);
         if (!ok) {
@@ -246,6 +258,20 @@ bool TelemetryDeviceDumper::open(yarp::os::Searchable& config) {
         return false;
     }
 
+    // Open RawValuesPublisherClient
+    if (settings.logIRawValuesPublisher) 
+    {
+        yarp::os::Property rawValPubClientProp{{"device", Value("rawValuesPublisherClient")},
+                                           {"remote", Value(settings.rawValuesPublisherRemoteName)}, //must have the name of the remote port defined in the related nws, i.e. RawValuesPublisherServer
+                                           {"local",  Value("/telemetryDeviceDumper" + settings.rawValuesPublisherRemoteName + "/client")}};
+        ok = this->rawValuesPublisherClient.open(rawValPubClientProp);
+        ok = ok && this->rawValuesPublisherClient.view(iravap);
+        if (!ok) {
+            yError() << "telemetryDeviceDumper: Problem opening the rawValuesPublisherClient.";
+            return false;
+        }
+    }
+
     ok = this->configBufferManager(config);
     if (!ok)
     {
@@ -309,10 +335,12 @@ bool TelemetryDeviceDumper::openRemapperControlBoard(yarp::os::Searchable& confi
 
     int axes = 0;
     ok = ok && remappedControlBoardInterfaces.encs->getAxes(&axes);
-    if (ok) {
+    if (ok) 
+    {
         this->resizeBuffers(axes);
     }
-    else {
+    else 
+    {
         yError() << "telemetryDeviceDumper: open impossible to use the necessary interfaces in remappedControlBoard";
         return ok;
     }
@@ -363,6 +391,7 @@ void TelemetryDeviceDumper::resizeBuffers(int size) {
     this->interactionModes.resize(size);
     // OdometryData has 9 fields
     this->odometryData.resize(9);
+    this->rawDataValuesMap.clear();
 
 }
 
@@ -412,7 +441,6 @@ bool TelemetryDeviceDumper::configBufferManager(yarp::os::Searchable& conf) {
         ok = ok && bufferManager.addChannel({ "odometry_data", {odometryData.size(), 1}, m_bufferConfig.description_list });
     }
 
-    ok = ok && bufferManager.configure(m_bufferConfig);
     // TODO check if we have nr of channels ~= 0
     return ok;
 }
@@ -423,6 +451,19 @@ bool TelemetryDeviceDumper::attachAll(const yarp::dev::PolyDriverList& device2at
     bool ok = true;
     ok = ok && this->attachAllControlBoards(device2attach);
 
+    if (ok && (settings.logIRawValuesPublisher))
+    {
+        // Configuring channels using metadata from interfaces
+        rawValuesKeyMetadataMap metadata = {}; // I just need to call it once while configuring (I think) 
+        iravap->getMetadataMap(metadata);
+        for (auto [k, m] : metadata.metadataMap)
+        {
+            ok = ok && bufferManager.addChannel({ "raw_data_values::"+k, {static_cast<uint16_t>(m.size), 1}, m.rawValueNames });
+        }
+    }
+
+    ok = ok && bufferManager.configure(m_bufferConfig);
+
     if (ok)
     {
         correctlyConfigured = true;
@@ -450,6 +491,12 @@ bool TelemetryDeviceDumper::close()
         localization2DClient.close();
     }
 
+    if (settings.logIRawValuesPublisher)
+    {
+        rawValuesPublisherClient.close();
+    }
+
+
     bool ok = true;
     if (settings.saveBufferManagerConfiguration) {
         auto buffConfToSave = bufferManager.getBufferConfig();
@@ -692,7 +739,7 @@ void TelemetryDeviceDumper::readOdometryData() {
     ok = iloc->getEstimatedOdometry(yarpOdomData);
     if (!ok)
     {
-        yWarning() << "telemetryDeviceDumper warning : odometry_data was not readed correctly";
+        yWarning() << "telemetryDeviceDumper warning : odometry_data was not read correctly";
     }
     else
     {
@@ -703,12 +750,36 @@ void TelemetryDeviceDumper::readOdometryData() {
     }
 }
 
+void TelemetryDeviceDumper::readRawValuesData()
+{
+    bool ok;
+    ok = iravap->getRawDataMap(rawDataValuesMap);
+    if (!ok)
+    {
+        yWarning() << "telemetryDeviceDumper warning : raw_data_values was not read correctly";
+    }
+    else
+    {
+        for (auto [key,value] : rawDataValuesMap)
+        {
+            bufferManager.push_back(value, "raw_data_values::"+key);
+        }
+    }
+
+}
+
 void TelemetryDeviceDumper::run() {
     if (correctlyConfigured) {
         readSensors();
         if (settings.logILocalization2D) {
             readOdometryData();
         }
+
+        if (settings.logIRawValuesPublisher)
+        {
+            readRawValuesData();
+        }
+
     }
     return;
 }

diff --git a/src/telemetryDeviceDumper/TelemetryDeviceDumper.h b/src/telemetryDeviceDumper/TelemetryDeviceDumper.h
@@ -19,6 +19,7 @@
 #include <yarp/dev/IControlMode.h>
 #include <yarp/dev/IInteractionMode.h>
 #include <yarp/dev/ILocalization2D.h>
+#include <iCub/IRawValuesPublisher.h>
 #include <yarp/dev/IMotor.h>
 #include <yarp/dev/ITorqueControl.h>
 #include <yarp/dev/PolyDriver.h>
@@ -27,6 +28,7 @@
 #include <robometry/BufferManager.h>
 
 #include <unordered_map>
+#include <map>
 #include <string>
 #include <memory>
 #include <vector>
@@ -47,9 +49,12 @@ struct TelemetryDeviceDumperSettings {
     bool logIPidControl{ false };
     bool logIAmplifierControl{ false };
     bool logILocalization2D{ false };
+    bool logIRawValuesPublisher { false };
     bool useRadians{ false };
     bool saveBufferManagerConfiguration{ false };
     std::string localizationRemoteName{ "" };
+    std::string rawValuesPublisherRemoteName { "" };
+
 };
 /**
  * @brief The `telemetryDeviceDumper` is a [yarp device](http://yarp.it/git-master/note_devices.html)
@@ -69,6 +74,7 @@ struct TelemetryDeviceDumperSettings {
  * | `logIAmplifierControl`     | bool     | -     | false     | No     | Enable the log of `motors_state::pwm` and `motors_state::currents` (http://yarp.it/git-master/classyarp_1_1dev_1_1IAmplifierControl.html).     |
  * | `logControlBoardQuantities` | bool     | -     | false     | No     | Enable the log of all the quantities that requires the attach to a control board (`logIEncoders`, `logITorqueControl`, `logIMotorEncoders`, `logIControlMode`, `logIInteractionMode`, `logIPidControl`, `logIAmplifierControl`). |
  * | `logILocalization2D` | bool     | -     | false     | No     | Enable the log of `odometry_data` (http://yarp.it/git-master/classyarp_1_1dev_1_1Nav2D_1_1ILocalization2D.html). |
+ * | `logIRawValuesPublisher` | bool |   -   | false    | No | Enable the log of `raw values` (https://github.com/robotology/icub-main/blob/devel/src/libraries/iCubDev/include/iCub/IRawValuesPublisher.h) |
  * | `saveBufferManagerConfiguration`     | bool     | -    | false     | No     | Enable the save of the configuration of the BufferManager into `path`+ `"bufferConfig"` + `experimentName` + `".json"`     |
  * | `json_file`     | string     | -     | -     | No     | Configure the `robometry::BufferManager`s reading from a json file like [in Example configuration file](#example-configuration-file). Note that this configuration will overwrite the parameter-by-parameter configuration   |
  * | `experimentName`     | string     | -     | -     | Yes     | Prefix of the files that will be saved. The files will be named: `experimentName`+`timestamp`+ `".mat"`.     |
@@ -99,7 +105,8 @@ struct TelemetryDeviceDumperSettings {
  * | `PIDs::torque_error`       | [`yarp::dev::IPidControl::getPidErrors`](http://yarp.it/git-master/classyarp_1_1dev_1_1IPidControl.html#aea29e0fdf34f819ac69a3b940556ba28) |
  * | `PIDs::torque_reference`   | [`yarp::dev::IPidControl::getPidReferences`](http://yarp.it/git-master/classyarp_1_1dev_1_1IPidControl.html#a29e8f684a15d859229a9ae2902f886da) |
  * | `PIDs::odometry_data   `   | [`yarp::dev::Nav2D::ILocalization2D::getEstimatedOdometry`](http://yarp.it/git-master/classyarp_1_1dev_1_1Nav2D_1_1ILocalization2D.html#a02bff57282777ce7511b671abd4c95f0) |
- *
+ * | `raw_data_values`          | [`iCub::debugLibrary::IRawValuesPublisher::getRawDataMap`] (https://github.com/robotology/icub-main/blob/devel/src/libraries/iCubDev/include/iCub/IRawValuesPublisher.h)
+ * 
  * @section Example_xml Example of xml
  *
  * Example of xml file for using it on the `iCub` robot:
@@ -118,6 +125,7 @@ struct TelemetryDeviceDumperSettings {
  *         <param name="logIInteractionMode">true</param>
  *         <param name="logIPidControl">false</param>
  *         <param name="logIAmplifierControl">true</param>
+ *         <param name="logIRawValuesPublisher">false</param>
  *         <param name="saveBufferManagerConfiguration">true</param>
  *         <param name="experimentName">test_telemetry</param>
  *         <param name="path">/home/icub/test_telemetry/</param>
@@ -182,10 +190,11 @@ class TelemetryDeviceDumper : public yarp::dev::DeviceDriver,
     bool openRemapperControlBoard(yarp::os::Searchable& config);
     void readSensors();
     void readOdometryData();
+    void readRawValuesData();
     void resizeBuffers(int size);
     bool configBufferManager(yarp::os::Searchable& config);
     /** Remapped controlboard containg the axes for which the joint torques are estimated */
-    yarp::dev::PolyDriver remappedControlBoard, localization2DClient;
+    yarp::dev::PolyDriver remappedControlBoard, localization2DClient, rawValuesPublisherClient;
     struct
     {
         yarp::dev::IEncoders* encs{nullptr};
@@ -200,12 +209,17 @@ class TelemetryDeviceDumper : public yarp::dev::DeviceDriver,
 
     yarp::dev::Nav2D::ILocalization2D* iloc{nullptr};
 
+    iCub::debugLibrary::IRawValuesPublisher* iravap{ nullptr };
+
     std::mutex deviceMutex;
     std::atomic<bool> correctlyConfigured{ false }, sensorsReadCorrectly{false};
     std::vector<double> jointPos, jointVel, jointAcc, jointPosErr, jointPosRef,
                         jointTrqErr, jointTrqRef, jointPWM, jointCurr, jointTrq,
                         motorEnc, motorVel, motorAcc, controlModes, interactionModes,
                         odometryData;
+
+    std::map<std::string, std::vector<std::int32_t>> rawDataValuesMap;
+
     std::vector<std::string> jointNames;
     TelemetryDeviceDumperSettings settings;
     robometry::BufferConfig m_bufferConfig;