TelemetryDeviceDumper: make the BM configurable from xml file

README.md

@@ -1,6 +1,7 @@
 ![YARP logo](https://raw.githubusercontent.com/robotology/yarp/master/doc/images/yarp-robot-24.png "YARP")
 YARP telemetry
 ==============
+
 :warning: LIBRARY UNDER DEVELOPMENT :warning:
 
 ![Continuous Integration](https://github.com/robotology-playground/yarp-telemetry/workflows/Continuous%20Integration/badge.svg)
@@ -9,6 +10,22 @@ YARP telemetry
 
 This is the telemetry component for YARP.
 
+## Table of contents
+- [Installation](#installation)
+  * [Dependencies](#dependencies)
+  * [Linux/macOS](#linux-macos)
+  * [Windows](#windows)
+- [libYARP_telemetry](#libyarp-telemetry)
+  * [Example scalar variable](#example-scalar-variable)
+  * [Example vector variable](#example-vector-variable)
+  * [Example matrix variable](#example-matrix-variable)
+  * [Example configuration file](#example-configuration-file)
+- [TelemetryDeviceDumper](#telemetrydevicedumper)
+  * [Parameters](#parameters)
+  * [Example of xml](#example-of-xml)
+- [Contributing](#contributing)
+- [License](#license)
+
 ## Installation
 ### Dependencies
 
@@ -46,7 +63,7 @@ cmake --build . --target INSTALL --config Release
 ```
 In order to allow CMake finding yarp-telemetry, you have to specify the path where you installed in the `CMAKE_PREFIX_PATH` or exporting the `YARP_telemetry_DIR` env variable pointing to the same path.
 
-## Usage
+## libYARP_telemetry
 In order to use this library in your own appliction add this lines in your `CMakeLists.txt`
 ```cmake
 find_package(YARP COMPONENTS telemetry)
@@ -240,6 +257,77 @@ 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 [`yarprobotointerface`](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.
+
+### Parameters
+
+
+
+| Parameter name | Type | Units | Default | Required | Description |
+| -------- | -------- | -------- | -------- | -------- | -------- |
+| `axesNames`     | List of strings     | -  | -     | Yes     | The axes contained in the axesNames parameter are then mapped to the wrapped controlboard in the attachAll method, using controlBoardRemapper class. |
+| `logJointVelocity`     | bool     | -     | false     | No     | Enable the log of joint velocities.     |
+| `logJointAcceleration`     | bool     | -     | false     | No     | Enable the log of joint accelerations.     |
+| `saveBufferManagerConfiguration`     | bool     | -    | false     | No     | Enable the save of the configuration of the BufferManager into `path`+ `"bufferConfig"` + `experimentName` + `".json"`     |
+| `json_file`     | string     | -     | -     | No     | Configure the `yarp::telemetry::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"`.     |
+| `path`     | string     | -     | -     | No     | Path of the folder where the data will be saved.     |
+| `n_samples`     | size_t     | -     | -     | Yes     | The max number of samples contained in the circular buffer/s     |
+| `save_periodically`     | bool     | -     | false     | No(but it has to be set to true if `auto_save` is set to false)     | The flag for enabling the periodic save thread.     |
+| `save_period`     | double     | seconds     | -     | Yes(if `save_periodically` is set to true)     | The period in seconds of the save thread     |
+| `data_threshold`     | size_t     | -     | 0     | No     | The save thread saves to a file if there are at least `data_threshold` samples     |
+| `auto_save`     | bool     | -     | false     | No(but it has to be set to true if `save_periodically` is set to false)     | the flag for enabling the save in the destructor of the `yarp::telemetry::BufferManager`     |
+
+
+
+### Example of xml
+
+Example of xml file for using it on the `iCub` robot:
+
+```xml
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="telemetryDeviceDumper" type="telemetryDeviceDumper">
+        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        <param name="logJointVelocity">true</param>
+        <param name="logJointAcceleration">true</param>
+        <param name="saveBufferManagerConfiguration">true</param>
+        <param name="experimentName">test_telemetry</param>
+        <param name="path">/home/icub/test_telemetry/</param>
+        <param name="n_samples">100000</param>
+        <param name="save_periodically">true</param>
+        <param name="save_period">120</param>
+        <param name="data_threshold">300</param>
+        <param name="auto_save">true</param>
+
+        <action phase="startup" level="15" type="attach">
+            <paramlist name="networks">
+                <!-- motorcontrol and virtual torque sensors -->
+                <elem name="left_lower_leg">left_leg-eb7-j4_5-mc</elem>
+                <elem name="right_lower_leg">right_leg-eb9-j4_5-mc</elem>
+                <elem name="left_upper_leg">left_leg-eb6-j0_3-mc</elem>
+                <elem name="right_upper_leg">right_leg-eb8-j0_3-mc</elem>
+                <elem name="torso">torso-eb5-j0_2-mc</elem>
+                <elem name="right_lower_arm">right_arm-eb27-j4_7-mc</elem>
+                <elem name="left_lower_arm">left_arm-eb24-j4_7-mc</elem>
+                <elem name="right_upper_arm">right_arm-eb3-j0_3-mc</elem>
+                <elem name="left_upper_arm">left_arm-eb1-j0_3-mc</elem>
+                <elem name="head-j0">head-eb20-j0_1-mc</elem>
+                <elem name="head-j2">head-eb21-j2_5-mc</elem>
+                <!-- ft -->
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="2" type="detach" />
+
+    </device>
+```
+
+
 
 ## Contributing
 

src/telemetryDeviceDumper/TelemetryDeviceDumper.cpp

@@ -87,14 +87,74 @@ bool TelemetryDeviceDumper::loadSettingsFromConfig(yarp::os::Searchable& config)
         settings.useRadians = prop.find(useRadians.c_str()).asBool();
     }
 
-    std::string experimentName = "experimentName";
-    if (prop.check(experimentName.c_str()) && prop.find(experimentName.c_str()).isString()) {
-        settings.experimentName = prop.find(experimentName.c_str()).asString();
+    std::string saveBufferManagerConfiguration = "saveBufferManagerConfiguration";
+    if (prop.check(saveBufferManagerConfiguration.c_str())) {
+        settings.saveBufferManagerConfiguration = prop.find(saveBufferManagerConfiguration.c_str()).asBool();
     }
 
-    std::string path = "path";
-    if (prop.check(path.c_str()) && prop.find(path.c_str()).isString()) {
-        settings.path = prop.find(path.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()) {
+        auto json_path = prop.find(json_file.c_str()).asString();
+        bool ok = bufferConfigFromJson(m_bufferConfig, json_path);
+        return ok;
+    }
+    else {
+        std::string experimentName = "experimentName";
+        if (prop.check(experimentName.c_str()) && prop.find(experimentName.c_str()).isString()) {
+            m_bufferConfig.filename = prop.find(experimentName.c_str()).asString();
+        }
+        else {
+            yError() << "TelemetryDeviceDumper: missing" << experimentName;
+            return false;
+        }
+
+        std::string path = "path";
+        if (prop.check(path.c_str()) && prop.find(path.c_str()).isString()) {
+            m_bufferConfig.path = prop.find(path.c_str()).asString();
+        }
+
+        std::string n_samples = "n_samples";
+        if (prop.check(n_samples.c_str()) && prop.find(n_samples.c_str()).isInt32()) {
+            m_bufferConfig.n_samples = prop.find(n_samples.c_str()).asInt32();
+        }
+        else {
+            yError() << "TelemetryDeviceDumper: missing" << n_samples;
+            return false;
+        }
+
+        std::string save_periodically = "save_periodically";
+        if (prop.check(save_periodically.c_str()) && prop.find(save_periodically.c_str()).isBool()) {
+            m_bufferConfig.save_periodically = prop.find(save_periodically.c_str()).asBool();
+        }
+
+        if (m_bufferConfig.save_periodically) {
+            std::string save_period = "save_period";
+            if (prop.check(save_period.c_str()) && prop.find(save_period.c_str()).isFloat64()) {
+                m_bufferConfig.save_period = prop.find(save_period.c_str()).asFloat64();
+            }
+            else {
+                yError() << "TelemetryDeviceDumper: missing" << save_period;
+                return false;
+            }
+
+            std::string data_threshold = "data_threshold";
+            if (prop.check(data_threshold.c_str()) && prop.find(data_threshold.c_str()).isInt32()) {
+                m_bufferConfig.data_threshold = prop.find(data_threshold.c_str()).asInt32();
+            }
+
+        }
+
+        std::string auto_save = "auto_save";
+        if (prop.check(auto_save.c_str()) && prop.find(auto_save.c_str()).isBool()) {
+            m_bufferConfig.auto_save = prop.find(auto_save.c_str()).asBool();
+        }
+
+    }
+
+    if (!(m_bufferConfig.auto_save || m_bufferConfig.save_periodically)) {
+        yError() << "TelemetryDeviceDumper: both auto_save and save_periodically are set to false, nothing will be saved.";
+        return false;
     }
 
     return true;
@@ -140,6 +200,10 @@ bool TelemetryDeviceDumper::openRemapperControlBoard(os::Searchable& config)
     bool ok = getUsedDOFsList(config, jointNames);
     if (!ok) return false;
 
+    // Add the joint names to the BufferManager
+    // Note that this will overwrite what is set from the configuration phase
+    m_bufferConfig.description_list = jointNames;
+
     addVectorOfStringToProperty(propRemapper, "axesNames", jointNames);
 
     ok = remappedControlBoard.open(propRemapper);
@@ -203,8 +267,6 @@ void TelemetryDeviceDumper::resizeBuffers(int size) {
 }
 
 bool TelemetryDeviceDumper::configBufferManager(yarp::os::Searchable& conf) {
-    // This is the buffer manager configuration
-    yarp::telemetry::BufferConfig bufferConfig;
     bool ok{ true };
     ok = ok && bufferManager.addChannel({ "encoders", {jointPos.size(), 1} });
     if (ok && settings.logJointVelocity) {
@@ -215,19 +277,7 @@ bool TelemetryDeviceDumper::configBufferManager(yarp::os::Searchable& conf) {
         ok = ok && bufferManager.addChannel({ "acceleration", {jointAcc.size(), 1} });
     }
 
-    // TODO: some settings from the ini are duplicated respect to the settings specified in the json file
-
-    bufferConfig.filename = settings.experimentName;
-    bufferConfig.path = settings.path;
-
-    // TODO for now it is just hardcoded
-    bufferConfig.n_samples = 100000;
-    bufferConfig.save_period = 120.0;
-    bufferConfig.data_threshold = 300;
-    bufferConfig.save_periodically = true;
-    bufferConfig.description_list = jointNames;
-
-    ok = ok && bufferManager.configure(bufferConfig);
+    ok = ok && bufferManager.configure(m_bufferConfig);
 
     return ok;
 }
@@ -263,8 +313,13 @@ bool TelemetryDeviceDumper::close()
     remappedControlBoard.close();
     // Flush all the remaining data.
     bufferManager.saveToFile();
+    bool ok = true;
+    if (settings.saveBufferManagerConfiguration) {
+        auto buffConfToSave = bufferManager.getBufferConfig();
+        ok = bufferConfigToJson(buffConfToSave, buffConfToSave.path + "bufferConfig" + buffConfToSave.filename + ".json");
+    }
 
-    return true;
+    return ok;
 }
 
 

src/telemetryDeviceDumper/TelemetryDeviceDumper.h

@@ -32,8 +32,7 @@ struct TelemetryDeviceDumperSettings {
     bool logJointVelocity{ false };
     bool logJointAcceleration{ false };
     bool useRadians{ false };
-    std::string experimentName{"telemetryDeviceDumper"};
-    std::string path{ "" };
+    bool saveBufferManagerConfiguration{ false };
 };
 /**
  * @brief FILL DOCUMENTATION
@@ -90,6 +89,7 @@ class TelemetryDeviceDumper : public yarp::dev::DeviceDriver,
     std::vector<double> jointPos, jointVel, jointAcc;
     std::vector<std::string> jointNames;
     TelemetryDeviceDumperSettings settings;
+    yarp::telemetry::BufferConfig m_bufferConfig;
     yarp::telemetry::BufferManager<double> bufferManager;
 
 

src/telemetryDeviceDumper/app/telelmetryDeviceDumper.xml

@@ -6,8 +6,14 @@
         <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         <param name="logJointVelocity">true</param>
         <param name="logJointAcceleration">true</param>
+        <param name="saveBufferManagerConfiguration">true</param>
         <param name="experimentName">test_telemetry</param>
         <param name="path">/home/icub/test_telemetry/</param>
+        <param name="n_samples">100000</param>
+        <param name="save_periodically">true</param>
+        <param name="save_period">120</param>
+        <param name="data_threshold">300</param>
+        <param name="auto_save">true</param>
 
         <action phase="startup" level="15" type="attach">
             <paramlist name="networks">