Robometry

Conda packages

It is possible to install on linux, macOS and Windows via conda, just running: conda install -c robotology robometry

Dependencies

The dependencies are:

• CMake (minimum version 3.12)
• Boost
• matio-cpp (minimum version 0.1.1)
• nlohmann_json (minimum version 3.10.0)
• Catch2 (v2.13.1, for the unit tests)

The optional dependencies are:

• YARP (minimum version 3.5.0)

Linux/macOS

git clone https://github.com/robotology/robometry
cd robometry
mkdir build && cd build
cmake ../
make
[sudo] make install

Notice: sudo is not necessary if you specify the CMAKE_INSTALL_PREFIX. In this case it is necessary to add in the .bashrc or .bash_profile the following lines:

export robometry_DIR=/path/where/you/installed/

Windows

With IDE build tool facilities, such as Visual Studio:

git clone https://github.com/robotology/robometry
cd robometry
mkdir build && cd build
cmake ..
cmake --build . --target ALL_BUILD --config Release
cmake --build . --target INSTALL --config Release

In order to allow CMake finding robometry, you have to specify the path where you installed in the CMAKE_PREFIX_PATH or exporting the robometry_DIR env variable pointing to the same path.

Example scalar variable

Here is the code snippet for dumping in a .mat file 3 samples of the scalar variables "one" and "two". The type of the channel is inferred when pushing the first time

{c++}
   robometry::BufferConfig bufferConfig;

   // We use the default config, setting only the number of samples (no auto/periodic saving)
   bufferConfig.n_samples = n_samples;

   robometry::BufferManager bm(bufferConfig);
   bm.setFileName("buffer_manager_test");
   robometry::ChannelInfo var_one{ "one", {1} };
   robometry::ChannelInfo var_two{ "two", {1} };

   bool ok = bm.addChannel(var_one);
   ok = ok && bm.addChannel(var_two);
   if (!ok) {
       std::cout << "Problem adding variables...."<<std::endl;
       return 1;
   }

   for (int i = 0; i < 10; i++) {
       bm.push_back(i , "one");
       std::this_thread::sleep_for(std::chrono::milliseconds(200));
       bm.push_back(i + 1.0, "two");
   }

   if (bm.saveToFile())
       std::cout << "File saved correctly!" << std::endl;
   else
       std::cout << "Something went wrong..." << std::endl;

And here is the resulting .mat file:

buffer_manager_test =

  struct with fields:

    description_list: {[1×0 char]}
                 two: [1×1 struct]
                 one: [1×1 struct]


buffer_manager_test.one =

  struct with fields:

              data: [1×3 int32]
        dimensions: [1 3]
    elements_names: {'element_0'}
              name: 'one'
        timestamps: [1.6481e+09 1.6481e+09 1.6481e+09]

Example vector variable

It is possible to save and dump also vector variables. Here is the code snippet for dumping in a .mat file 3 samples of the 4x1 vector variables "one" and "two".

{c++}
   robometry::BufferConfig bufferConfig;
   bufferConfig.auto_save = true; // It will save when invoking the destructor
   bufferConfig.channels = { {"one",{4,1}}, {"two",{4,1}} };
   bufferConfig.filename = "buffer_manager_test_vector";
   bufferConfig.n_samples = 3;

   robometry::BufferManager bm_v(bufferConfig); //Only vectors of doubles are accepted
   for (int i = 0; i < 10; i++) {
       bm_v.push_back({ i+1.0, i+2.0, i+3.0, i+4.0  }, "one");
       std::this_thread::sleep_for(std::chrono::milliseconds(200));
       bm_v.push_back({ (double)i, i*2.0, i*3.0, i*4.0 }, "two");
   }

buffer_manager_test_vector =

  struct with fields:

    description_list: {[1×0 char]}
                 two: [1×1 struct]
                 one: [1×1 struct]


>> buffer_manager_test_vector.one

ans =

  struct with fields:

              data: [4×1×3 double]
        dimensions: [4 1 3]
    elements_names: {4×1 cell}
              name: 'one'
        timestamps: [1.6481e+09 1.6481e+09 1.6481e+09]


>> buffer_manager_test_vector.one.elements_names

ans =

  4×1 cell array

    {'element_0'}
    {'element_1'}
    {'element_2'}
    {'element_3'}

It is also possible to specify the name of the elements of each variable with

 {c++}
robometry::ChannelInfo var_one{ "one", {4,1}, {"A", "B", "C", "D"}};

Example matrix variable

Here is the code snippet for dumping in a .mat file 3 samples of the 2x3 matrix variable"one" and of the 3x2 matrix variable "two". BufferManager expects all the inputs to be of vector types, but then input is remapped into a matrix of the specified type.

{c++}
   robometry::BufferManager bm_m;
   bm_m.resize(3);
   bm_m.setFileName("buffer_manager_test_matrix");
   bm_m.enablePeriodicSave(0.1); // This will try to save a file each 0.1 sec
   bm_m.setDefaultPath("/my/preferred/path");
   bm_m.setDescriptionList({"head", "left_arm"});
   std::vector<robometry::ChannelInfo> vars{ { "one",{2,3} },
                                                   { "two",{3,2} } };

   bool ok = bm_m.addChannels(vars);
   if (!ok) {
       std::cout << "Problem adding variables...."<<std::endl;
       return 1;
   }

   for (int i = 0; i < 10; i++) {
       bm_m.push_back({ i + 1, i + 2, i + 3, i + 4, i + 5, i + 6 }, "one");
       std::this_thread::sleep_for(std::chrono::milliseconds(200));
       bm_m.push_back({ i * 1, i * 2, i * 3, i * 4, i * 5, i * 6 }, "two");
   }

>> buffer_manager_test_matrix.one

ans =

  struct with fields:

          data: [2×3×3 int32]
    dimensions: [2 3 3]
          name: 'one'
    timestamps: [112104.7605783 112104.9608881 112105.1611651]

Example nested struct

It is possible to save and dump vectors and matrices into nested mat structures. To add an element into the matlab struct the you should use the separator ::. For instance the to store a vector in A.B.C.my_vector you should define the channel name as A::B::C::my_vector Here is the code snippet for dumping in a .mat file 3 samples of the 4x1 vector variables "one" and "two" into struct1 and struct2.

{c++}
   robometry::BufferConfig bufferConfig;
   bufferConfig.auto_save = true; // It will save when invoking the destructor
   bufferConfig.channels = { {"struct1::one",{4,1}}, {"struct1::two",{4,1}}, {"struct2::one",{4,1}} }; // Definition of the elements into substruct
   bufferConfig.filename = "buffer_manager_test_nested_vector";
   bufferConfig.n_samples = 3;

   robometry::BufferManager bm_v(bufferConfig);
   for (int i = 0; i < 10; i++) {
       bm_v.push_back({ i+1.0, i+2.0, i+3.0, i+4.0  }, "struct1::one");
       std::this_thread::sleep_for(std::chrono::milliseconds(200));
       bm_v.push_back({ (double)i, i*2.0, i*3.0, i*4.0 }, "struct1::two");
       std::this_thread::sleep_for(std::chrono::milliseconds(200));
       bm_v.push_back({ (double)i, i/2.0, i/3.0, i/4.0 }, "struct2::one");
   }

buffer_manager_test_nested_vector =

  struct with fields:

    description_list: {[1×0 char]}
             struct2: [1×1 struct]
             struct1: [1×1 struct]

>> buffer_manager_test_nested_vector.struct1

ans =

  struct with fields:

    two: [1×1 struct]
    one: [1×1 struct]

>> buffer_manager_test_nested_vector.struct1.one

ans =

  struct with fields:

          data: [4×1×3 double]
    dimensions: [4 1 3]
          name: 'one'
    timestamps: [1.6415e+09 1.6415e+09 1.6415e+09]

Example multiple types

BufferManager can be used to store channels of different types, including structs. In order to store a struct, it is necessary to use the VISITABLE_STRUCT macro (see https://github.com/garbageslam/visit_struct). The available conversions depend on matio-cpp.

 {c++}
struct testStruct
{
    int a;
    double b;
};
VISITABLE_STRUCT(testStruct, a, b);

...

    robometry::BufferManager bm;
    robometry::BufferConfig bufferConfig;

    robometry::ChannelInfo var_int{ "int_channel", {1}};
    robometry::ChannelInfo var_double{ "double_channel", {1}};
    robometry::ChannelInfo var_string{ "string_channel", {1}};
    robometry::ChannelInfo var_vector{ "vector_channel", {4, 1}};
    robometry::ChannelInfo var_struct{ "struct_channel", {1}};

    bm.addChannel(var_int);
    bm.addChannel(var_double);
    bm.addChannel(var_string);
    bm.addChannel(var_vector);
    bm.addChannel(var_struct);

    bufferConfig.n_samples = 3;
    bufferConfig.filename = "buffer_manager_test_multiple_types";
    bufferConfig.auto_save = true;

    bm.configure(bufferConfig);

    testStruct item;

    for (int i = 0; i < 10; i++) {
        bm.push_back(i, "int_channel");
        bm.push_back(i * 1.0, "double_channel");
        bm.push_back("iter" + std::to_string(i), "string_channel");
        bm.push_back({i + 0.0, i + 1.0, i + 2.0, i + 3.0}, "vector_channel");
        item.a = i;
        item.b = i;
        bm.push_back(item, "struct_channel");

        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

The above snippet of code generates channels of different types. It produces the following output.

>> buffer_manager_test_multiple_types

buffer_manager_test_multiple_types =

  struct with fields:

    description_list: {[1×0 char]}
     yarp_robot_name: [1×0 char]
      struct_channel: [1×1 struct]
      vector_channel: [1×1 struct]
      string_channel: [1×1 struct]
      double_channel: [1×1 struct]
         int_channel: [1×1 struct]

>> buffer_manager_test_multiple_types.string_channel

ans =

  struct with fields:

              data: {1×3 cell}
        dimensions: [1 3]
    elements_names: {'element_0'}
              name: 'string_channel'
        timestamps: [1.6512e+09 1.6512e+09 1.6512e+09]

>> buffer_manager_test_multiple_types.vector_channel

ans =

  struct with fields:

              data: [4×1×3 double]
        dimensions: [4 1 3]
    elements_names: {4×1 cell}
              name: 'vector_channel'
        timestamps: [1.6512e+09 1.6512e+09 1.6512e+09]

Example additional callback

BufferManager can call an additional callback every time the save function is called. The following example define a custom callback that saves a dummy txt file along with the mat saved by the telemetry

 {c++}
bool myCallback(const std::string& file_name, const SaveCallbackSaveMethod& method) {
  std::string file_name_with_extension = file_name + ".txt";
  std::ofstream my_file(file_name_with_extension.c_str());

  // Write to the file
  my_file << "Dummy file!";

  // Close the file
  my_file.close();

  return true;
};


robometry::BufferManager bm;
bm.setSaveCallback(myCallback);

Example configuration file

It is possible to load the configuration of a BufferManager from a json file

{c++}
  robometry::BufferManager bm;
  robometry::BufferConfig bufferConfig;
  bool ok = bufferConfigFromJson(bufferConfig,"test_json.json");
  ok = ok && bm.configure(bufferConfig);

Where the file has to have this format:

{
  "yarp_robot_name": "robot",
  "description_list": [
    "This is a test",
    "Or it isn't?"
  ],
  "path":"/my/preferred/path",
  "filename": "buffer_manager_test_conf_file",
  "n_samples": 20,
  "save_period": 1.0,
  "data_threshold": 10,
  "auto_save": true,
  "save_periodically": true,
  "channels": [
    {
      "dimensions": [1,1],
      "elements_names": ["element_0"],
      "name": "one"
    },
    {
      "dimensions": [1,1],
      "elements_names": ["element_0"],
      "name": "two"
    }
  ],
  "enable_compression": true,
  "file_indexing": "%Y_%m_%d_%H_%M_%S",
  "mat_file_version": "v7_3"
}

The configuration can be saved to a json file

{c++}
   robometry::BufferConfig bufferConfig;
   bufferConfig.n_samples = 10;
   bufferConfig.save_period = 0.1; //seconds
   bufferConfig.data_threshold = 5;
   bufferConfig.save_periodically = true;
   std::vector<robometry::ChannelInfo> vars{ { "one",{2,3} },
                                                   { "two",{3,2} } };
   bufferConfig.channels = vars;

   auto ok = bufferConfigToJson(bufferConfig, "test_json_write.json");

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 enviromental variable (for more on the YARP_DATA_DIRS env variable, see YARP documentation on data directories ).

• Once you do that, you should be able to find the telemetryDeviceDumper device compiled by this repo using the command yarp plugin telemetryDeviceDumper, which should have an output similar to:

  Yes, this is a YARP plugin
    * library:        CMAKE_INSTALL_PREFIX/lib/yarp/yarp_telemetryDeviceDumper.dll
    * system version: 5
    * class name:     robometry::TelemetryDeviceDumper
    * base class:     yarp::dev::DeviceDriver

  If this is not the case, there could be some problems in finding the plugin. In that case, just move yourself to the ${CMAKE_INSTALL_PREFIX}/share/yarp directory and launch the device from there.

Parameters

Mapping .mat variables -> YARP interfaces

Example of xml

Example of xml file for using it on the iCub robot:

<?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="logIEncoders">true</param>
        <param name="logITorqueControl">true</param>
        <param name="logIMotorEncoders">true</param>
        <param name="logIControlMode">true</param>
        <param name="logIInteractionMode">true</param>
        <param name="logIPidControl">false</param>
        <param name="logIAmplifierControl">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.0</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>