├── .clang-format ├── .clang-tidy ├── .github └── workflows │ ├── ci.yml │ ├── conda-ci.yml │ └── gh-pages.yml ├── .gitignore ├── CHANGELOG.md ├── CMakeLists.txt ├── LICENSE ├── README.md ├── docs ├── Doxyfile-mcss.in ├── Doxyfile.in ├── conf-master.py ├── conf.py.in ├── config.toml ├── generate_documentation_files.py ├── generate_website.py └── pages │ └── magnitude_frequency_table.md ├── material └── slides │ ├── app-logger.pptx │ ├── present-logging.pptx │ ├── telemetry_user_case.pptx │ ├── yarp-telemetry-20201014.pdf │ └── yarp-telemetry-api.pdf ├── src ├── CMakeLists.txt ├── examples │ ├── CB_to_matfile_example.cpp │ ├── CMakeLists.txt │ ├── circular_buffer_example.cpp │ ├── circular_buffer_record_example.cpp │ ├── conf │ │ └── test_json.json │ ├── matio_matrix_example.cpp │ ├── matio_timeseries_example.cpp │ ├── matio_vector_example.cpp │ ├── robometry_buffer_example.cpp │ ├── robometry_buffer_manager_conf_file.cpp │ ├── robometry_buffer_manager_example.cpp │ └── robometry_buffer_periodic_save.cpp ├── librobometry │ ├── CMakeLists.txt │ ├── include │ │ └── robometry │ │ │ ├── Buffer.h │ │ │ ├── BufferConfig.h │ │ │ ├── BufferManager.h │ │ │ ├── Record.h │ │ │ └── TreeNode.h │ └── src │ │ ├── Buffer.cpp │ │ ├── BufferConfig.cpp │ │ └── BufferManager.cpp └── telemetryDeviceDumper │ ├── CMakeLists.txt │ ├── TelemetryDeviceDumper.cpp │ ├── TelemetryDeviceDumper.h │ └── app │ ├── launch-telemetryDeviceDumper-icub_sim.xml │ ├── telemetryDeviceDumper.xml │ ├── telemetryDeviceDumper_sim.xml │ └── yarprobotinterface.ini └── test ├── BufferManagerTest.cpp └── CMakeLists.txt /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: WebKit 4 | AccessModifierOffset: -4 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveMacros: false 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveBitFields: false 9 | AlignConsecutiveDeclarations: false 10 | AlignEscapedNewlines: Right 11 | AlignOperands: AlignAfterOperator 12 | AlignTrailingComments: true 13 | AllowAllArgumentsOnNextLine: false 14 | AllowAllConstructorInitializersOnNextLine: false 15 | AllowAllParametersOfDeclarationOnNextLine: false 16 | AllowShortBlocksOnASingleLine: Empty 17 | AllowShortCaseLabelsOnASingleLine: false 18 | AllowShortEnumsOnASingleLine: false 19 | AllowShortFunctionsOnASingleLine: None 20 | AllowShortLambdasOnASingleLine: All 21 | AllowShortIfStatementsOnASingleLine: Never 22 | AllowShortLoopsOnASingleLine: false 23 | # AlwaysBreakAfterDefinitionReturnType: None # Deprecated 24 | AlwaysBreakAfterReturnType: None 25 | AlwaysBreakBeforeMultilineStrings: false 26 | AlwaysBreakTemplateDeclarations: Yes 27 | BinPackArguments: false 28 | BinPackParameters: false 29 | BraceWrapping: 30 | AfterCaseLabel: false 31 | AfterClass: true 32 | AfterControlStatement: Never 33 | AfterEnum: true 34 | AfterFunction: true 35 | AfterNamespace: false 36 | AfterObjCDeclaration: false 37 | AfterStruct: true 38 | AfterUnion: true 39 | AfterExternBlock: false 40 | BeforeCatch: false 41 | BeforeElse: false 42 | BeforeLambdaBody: true 43 | BeforeWhile: false 44 | IndentBraces: false 45 | SplitEmptyFunction: true 46 | SplitEmptyRecord: true 47 | SplitEmptyNamespace: true 48 | BreakBeforeBinaryOperators: All 49 | BreakBeforeBraces: Custom 50 | BreakBeforeInheritanceComma: false 51 | BreakInheritanceList: AfterColon 52 | BreakBeforeTernaryOperators: true 53 | BreakConstructorInitializersBeforeComma: false 54 | BreakConstructorInitializers: AfterColon 55 | BreakAfterJavaFieldAnnotations: false 56 | BreakStringLiterals: true 57 | ColumnLimit: 0 58 | CommentPragmas: '^ IWYU pragma:' 59 | CompactNamespaces: false 60 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 61 | ConstructorInitializerIndentWidth: 8 62 | ContinuationIndentWidth: 4 63 | Cpp11BracedListStyle: true 64 | DeriveLineEnding: false 65 | DerivePointerAlignment: false 66 | DisableFormat: false 67 | ExperimentalAutoDetectBinPacking: false 68 | FixNamespaceComments: false 69 | ForEachMacros: 70 | - foreach 71 | - Q_FOREACH 72 | - BOOST_FOREACH 73 | IncludeBlocks: Regroup 74 | IncludeCategories: 75 | - Regex: '^ [!warning] 8 | > This file documents notable changes to this project done before December 2024. 9 | > For changes after that date, please take a look at the release notes of each release at https://github.com/robotology/robometry/releases. 10 | 11 | 12 | ## [1.2.5] - 2024-12-06 13 | 14 | - Added to TelemetryDeviceDumper streaming of rawValues from low level (#190) 15 | 16 | ## [1.2.4] - 2024-05-22 17 | - Added error message in case a leaf name is empty 18 | 19 | ## [1.2.3] - 2024-02-28 20 | 21 | - Added `log_period` parameter 22 | 23 | ## [1.2.2] - 2023-09-06 24 | 25 | - Ported unit tests to catch v3. 26 | 27 | ## [1.2.0] - 2022-08-31 28 | 29 | - Added `units_of_measure` parameter. 30 | 31 | ## [1.1.0] - 2022-06-09 32 | 33 | - Added doxygen documentation available at https://robotology.github.io/robometry/index.html. 34 | - Removed deprecated options `logAllQuantities`, `logJointAcceleration`, `logJointVelocity` 35 | from `telemetryDeviceDumper` device. 36 | 37 | ## [1.0.0] - 2022-05-23 38 | - Renamed the repo robometry instead of yarp-telemetry. 39 | - Renamed `YARP_telemetry` in `robometry`. 40 | - ``BufferManager`` is no more a templated class. 41 | 42 | ## [0.5.1] - 2022-05-06 43 | 44 | - Added the method `setSaveCallback` for adding an additiona function invoked with 45 | `saveToFile`, for saving additional data (e.g. videos). 46 | 47 | ## [0.5.0] - 2022-05-04 48 | 49 | - Add `yarp_robot_name` variable in the saved mat file 50 | - Added the possibility to specify the names of the each element of a channel. 51 | - BufferInfo is now a struct that contains the name, the dimension and the elements_names. 52 | - Added the possibility to have channels with different types (including custom structs) in a single ``BufferManager``. 53 | - Fixed a bug preventing a log file to be saved when multiple channels are empty. 54 | - YARP is now an optional dependency 55 | - Added `yarp_robot_name` in `telemetryDeviceDumper`. 56 | - Refactored variables names in `telemetryDeviceDumper` in a more hierarchical structure. 57 | 58 | ## [0.4.0] - 2022-02-22 59 | 60 | - Added the possibility to pass `matioCpp::Span` objects to `BufferManager::push_back` and `Record` constructors 61 | - Added the creation of the dir in the `BufferManager` if the path specified does not exist. 62 | - Added the `file_indexing` parameter in the `BufferConfig` struct. 63 | - Add the possibility to specify the saved matfile version in the `BufferManager` class. 64 | - Added the possibility to have multilayer structures in the `BufferManager`. 65 | 66 | ## [0.3.0] - 2021-10-18 67 | 68 | - Added the log of the estimated odometry from `yarp::dev::Nav2D::ILocalization2D`. 69 | - Deprecated the `logAllQuantities` option in favour of `logControlBoardQuantities`. 70 | 71 | ## [0.2.0] - 2021-05-19 72 | 73 | - Added the possibility to specify the time of a ``Record`` with ``push_back``. 74 | - Added the possibility to enable the zlib compression. 75 | - Fixed yarp-telemetry.ini generation. 76 | - Fixed load of `telemetryDeviceDumper` plugin. 77 | - Added the log from these interfaces in the `telemetryDeviceDumper`: 78 | - `yarp::dev::IMotorEncoders` 79 | - `yarp::dev::IPidControl` 80 | - `yarp::dev::IAmplifierControl` 81 | - `yarp::dev::IControlMode` 82 | - `yarp::dev::IInteractionMode` 83 | - `yarp::dev::ITorqueControl` 84 | - Added check of the existence of the path specified in the configuration. 85 | 86 | ## [0.1.0] - 2021-04-02 87 | 88 | First release of `yarp-telemetry`, compatible with YARP 3.4. 89 | 90 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | cmake_minimum_required(VERSION 3.16) 8 | project(robometry LANGUAGES C CXX 9 | VERSION 1.2.7) 10 | 11 | include(GNUInstallDirs) 12 | include(FeatureSummary) 13 | 14 | find_package(YCM 0.12 REQUIRED) 15 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 16 | 17 | set(YARP_REQUIRED_VERSION 3.5.0) 18 | 19 | # Control where libraries and executables are placed during the build. 20 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") 21 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") 22 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") 23 | 24 | # Under MSVC, we set CMAKE_DEBUG_POSTFIX to "d" to add a trailing "d" to library 25 | # built in debug mode. 26 | if(MSVC) 27 | set(CMAKE_DEBUG_POSTFIX "d") 28 | endif() 29 | 30 | # To build shared libraries in Windows, we set CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS to TRUE. 31 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 32 | 33 | option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) 34 | # Build position independent code. 35 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 36 | 37 | # Disable C and C++ compiler extensions. 38 | set(CMAKE_C_EXTENSIONS OFF) 39 | set(CMAKE_CXX_EXTENSIONS OFF) 40 | 41 | if(${CMAKE_VERSION} VERSION_GREATER_EQUAL 3.19) 42 | cmake_policy(SET CMP0111 NEW) 43 | endif() 44 | 45 | 46 | # Enable RPATH support for installed binaries and libraries 47 | include(AddInstallRPATHSupport) 48 | add_install_rpath_support(LIB_DIRS "${CMAKE_INSTALL_FULL_LIBDIR}" # Libraries 49 | BIN_DIRS "${CMAKE_INSTALL_FULL_BINDIR}" # Binaries 50 | "${CMAKE_INSTALL_FULL_LIBDIR}/yarp" # Plugins 51 | INSTALL_NAME_DIR "${CMAKE_INSTALL_FULL_LIBDIR}" 52 | USE_LINK_PATH) 53 | 54 | # Encourage user to specify a build type (e.g. Release, Debug, etc.), otherwise set it to Release. 55 | if(NOT CMAKE_CONFIGURATION_TYPES) 56 | if(NOT CMAKE_BUILD_TYPE) 57 | message(STATUS "Setting build type to 'Release' as none was specified.") 58 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release") 59 | endif() 60 | endif() 61 | 62 | #### Dependencies 63 | find_package(matioCpp 0.2.0 REQUIRED) 64 | find_package(Boost REQUIRED) 65 | find_package(Threads REQUIRED) 66 | 67 | #### Optional Dependencies 68 | find_package(YARP ${YARP_REQUIRED_VERSION} COMPONENTS conf os dev QUIET) 69 | set(YARP_FORCE_DYNAMIC_PLUGINS TRUE CACHE INTERNAL "${PROJECT_NAME} is always built with dynamic plugins") 70 | find_package(iCubDev 2.7.0 QUIET) 71 | 72 | option(ROBOMETRY_USES_SYSTEM_nlohmann_json OFF) 73 | 74 | if(ROBOMETRY_USES_SYSTEM_nlohmann_json) 75 | find_package(nlohmann_json 3.10.0 REQUIRED) 76 | else() 77 | include(FetchContent) 78 | 79 | FetchContent_Declare(json 80 | GIT_REPOSITORY https://github.com/nlohmann/json.git 81 | GIT_TAG v3.11.3) 82 | 83 | FetchContent_GetProperties(json) 84 | if(NOT json_POPULATED) 85 | message(STATUS "Fetching nlohmann_json...") 86 | FetchContent_Populate(json) 87 | add_subdirectory(${json_SOURCE_DIR} ${json_BINARY_DIR} EXCLUDE_FROM_ALL) 88 | endif() 89 | endif() 90 | 91 | 92 | feature_summary(WHAT ALL INCLUDE_QUIET_PACKAGES) 93 | 94 | add_subdirectory(src) 95 | 96 | option(BUILD_EXAMPLES "Build the examples" ON) 97 | option(BUILD_TESTING "Create tests using CMake" OFF) 98 | option(ROBOMETRY_USES_SYSTEM_Catch2 OFF) 99 | 100 | ############################## 101 | ########### Test ############# 102 | ############################## 103 | include(CMakeDependentOption) 104 | cmake_dependent_option(ROBOMETRY_VALGRIND_TESTS 105 | "Run tests under Valgrind" OFF 106 | "BUILD_TESTING" OFF) 107 | cmake_dependent_option(ROBOMETRY_BENCHMARKING 108 | "Enable the benchmarking in the unit tests" OFF 109 | "BUILD_TESTING" OFF) 110 | mark_as_advanced(ROBOMETRY_VALGRIND_TESTS) 111 | 112 | if(ROBOMETRY_VALGRIND_TESTS) 113 | find_program(VALGRIND_EXECUTABLE NAMES valgrind) 114 | mark_as_advanced(VALGRIND_EXECUTABLE) 115 | 116 | if(VALGRIND_EXECUTABLE) 117 | set(VALGRIND_OPTIONS "--tool=memcheck --leak-check=full" 118 | CACHE STRING "Valgrind options (--error-exitcode=1 will be appended)") 119 | mark_as_advanced(VALGRIND_OPTIONS) 120 | separate_arguments(VALGRIND_OPTIONS UNIX_COMMAND "${VALGRIND_OPTIONS}") 121 | set(VALGRIND_COMMAND "${VALGRIND_EXECUTABLE}" ${VALGRIND_OPTIONS} --error-exitcode=1 --fullpath-after=${CMAKE_SOURCE_DIR}/) 122 | else() 123 | message(SEND_ERROR "Valgrind executable not found") 124 | endif() 125 | endif() 126 | 127 | set(ROBOMETRY_TEST_TIMEOUT_DEFAULT_VALGRIND 300) 128 | if(DEFINED VALGRIND_COMMAND) 129 | set(ROBOMETRY_TEST_LAUNCHER ${VALGRIND_COMMAND}) 130 | endif() 131 | set(ROBOMETRY_TEST_TIMEOUT ${ROBOMETRY_TEST_TIMEOUT_DEFAULT_VALGRIND}) 132 | 133 | if(BUILD_TESTING) 134 | 135 | if(ROBOMETRY_USES_SYSTEM_Catch2) 136 | find_package(Catch 3.7.1 REQUIRED) 137 | else() 138 | include(FetchContent) 139 | 140 | message(STATUS "Fetching Catch...") 141 | 142 | FetchContent_Declare(Catch 143 | GIT_REPOSITORY https://github.com/catchorg/Catch2.git 144 | GIT_TAG v3.7.1) 145 | 146 | FetchContent_MakeAvailable(Catch) 147 | endif() 148 | enable_testing() 149 | add_subdirectory(test) 150 | endif() 151 | 152 | set_property(GLOBAL PROPERTY USE_FOLDERS 1) 153 | 154 | include(AddUninstallTarget) 155 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) Fondazione Istituto Italiano di Tecnologia 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions 6 | are met: 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | 3. Neither the name of the University nor the names of its contributors 13 | may be used to endorse or promote products derived from this software 14 | without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22 | OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25 | OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26 | SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | Robometry 3 | ========= 4 | 5 | ![Continuous Integration](https://github.com/robotology/robometry/workflows/CI%20Workflow/badge.svg) 6 | 7 | [![Anaconda-Server Badge](https://anaconda.org/robotology/robometry/badges/downloads.svg)](https://anaconda.org/conda-forge/librobometry) 8 | [![Anaconda-Server Badge](https://anaconda.org/robotology/robometry/badges/installer/conda.svg)](https://anaconda.org/conda-forge/librobometry) 9 | [![Anaconda-Server Badge](https://anaconda.org/robotology/robometry/badges/platforms.svg)](https://anaconda.org/conda-forge/librobometry) 10 | 11 | Telemetry suite for logging data from your robot 🤖. 12 | 13 | ## Tested OSes 14 | - Windows 10 15 | - Ubuntu 20.04, 22.04 16 | - macOS >= 10.15 17 | 18 | ## Installation from binaries 19 | 20 | ### Conda packages 21 | 22 | It is possible to install on `linux`, `macOS` and `Windows` via [conda](https://anaconda.org/conda-forge/librobometry), just running: 23 | ```bash 24 | conda install -c conda-forge librobometry 25 | ``` 26 | 27 | ## Installation from sources 28 | 29 | ### Dependencies 30 | 31 | The dependencies are: 32 | - [CMake](https://cmake.org/install/) (minimum version 3.12) 33 | - [Boost](https://www.boost.org/) 34 | - [matio-cpp](https://github.com/ami-iit/matio-cpp#installation) (minimum version 0.1.1) 35 | - [nlohmann_json](https://github.com/nlohmann/json#integration) (minimum version 3.10.0) 36 | - [Catch2](https://github.com/catchorg/Catch2.git) (v3.7.1, for the unit tests) 37 | 38 | The optional dependencies are: 39 | - [YARP](https://www.yarp.it/git-master/install.html) (minimum version 3.5.0) 40 | - [icub-main](https://robotology.github.io/robotology-documentation/doc/html/index.html) (minimum version 2.7.0) 41 | 42 | ### Linux/macOS 43 | ``` 44 | git clone https://github.com/robotology/robometry 45 | cd robometry 46 | mkdir build && cd build 47 | cmake ../ 48 | make 49 | [sudo] make install 50 | ``` 51 | 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: 52 | 53 | `export robometry_DIR=/path/where/you/installed/` 54 | 55 | ### Windows 56 | 57 | With IDE build tool facilities, such as Visual Studio: 58 | ``` 59 | git clone https://github.com/robotology/robometry 60 | cd robometry 61 | mkdir build && cd build 62 | cmake .. 63 | cmake --build . --target ALL_BUILD --config Release 64 | cmake --build . --target INSTALL --config Release 65 | ``` 66 | 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. 67 | 68 | ## librobometry 69 | In order to use this library in your own application, add this lines in your `CMakeLists.txt` 70 | ```cmake 71 | find_package(robometry) 72 | 73 | add_executable(myApp) 74 | target_link_libraries(myApp robometry::robometry) 75 | ``` 76 | 77 | ### Example scalar variable 78 | 79 | 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 80 | 81 | ```c++ 82 | robometry::BufferConfig bufferConfig; 83 | 84 | // We use the default config, setting only the number of samples (no auto/periodic saving) 85 | bufferConfig.n_samples = n_samples; 86 | 87 | robometry::BufferManager bm(bufferConfig); 88 | bm.setFileName("buffer_manager_test"); 89 | robometry::ChannelInfo var_one{ "one", {1} }; 90 | robometry::ChannelInfo var_two{ "two", {1} }; 91 | 92 | bool ok = bm.addChannel(var_one); 93 | ok = ok && bm.addChannel(var_two); 94 | if (!ok) { 95 | std::cout << "Problem adding variables...."<> buffer_manager_test_vector.one 167 | 168 | ans = 169 | 170 | struct with fields: 171 | 172 | data: [4×1×3 double] 173 | dimensions: [4 1 3] 174 | elements_names: {4×1 cell} 175 | units_of_measure: {4×1 cell} 176 | name: 'one' 177 | timestamps: [1.6481e+09 1.6481e+09 1.6481e+09] 178 | 179 | 180 | >> buffer_manager_test_vector.one.elements_names 181 | 182 | ans = 183 | 184 | 4×1 cell array 185 | 186 | {'element_0'} 187 | {'element_1'} 188 | {'element_2'} 189 | {'element_3'} 190 | 191 | >> buffer_manager_test_vector.one.units_of_measure 192 | 193 | ans = 194 | 195 | 4×1 cell array 196 | 197 | {'m'} 198 | {'m'} 199 | {'m'} 200 | {'m'} 201 | ``` 202 | 203 | It is also possible to specify the name of the elements of each variable with 204 | ```c++ 205 | robometry::ChannelInfo var_one{ "one", {4,1}, {"A", "B", "C", "D"}, {"m", "cm", "mm", "nm"}}; 206 | ``` 207 | 208 | 209 | ### Example matrix variable 210 | 211 | 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"`. 212 | ``BufferManager`` expects all the inputs to be of vector types, but then input is remapped into a matrix of the specified type. 213 | 214 | ```c++ 215 | robometry::BufferManager bm_m; 216 | bm_m.resize(3); 217 | bm_m.setFileName("buffer_manager_test_matrix"); 218 | bm_m.enablePeriodicSave(0.1); // This will try to save a file each 0.1 sec 219 | bm_m.setDefaultPath("/my/preferred/path"); 220 | bm_m.setDescriptionList({"head", "left_arm"}); 221 | std::vector vars{ { "one",{2,3} }, 222 | { "two",{3,2} } }; 223 | 224 | bool ok = bm_m.addChannels(vars); 225 | if (!ok) { 226 | std::cout << "Problem adding variables...."<> buffer_manager_test_matrix.one 240 | 241 | ans = 242 | 243 | struct with fields: 244 | 245 | data: [2×3×3 int32] 246 | dimensions: [2 3 3] 247 | name: 'one' 248 | timestamps: [112104.7605783 112104.9608881 112105.1611651] 249 | 250 | ``` 251 | ### Example nested struct 252 | 253 | 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` 254 | 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`. 255 | 256 | ```c++ 257 | robometry::BufferConfig bufferConfig; 258 | bufferConfig.auto_save = true; // It will save when invoking the destructor 259 | bufferConfig.channels = { {"struct1::one",{4,1}}, {"struct1::two",{4,1}}, {"struct2::one",{4,1}} }; // Definition of the elements into substruct 260 | bufferConfig.filename = "buffer_manager_test_nested_vector"; 261 | bufferConfig.n_samples = 3; 262 | 263 | robometry::BufferManager bm_v(bufferConfig); 264 | for (int i = 0; i < 3; i++) { 265 | bm_v.push_back({ i+1.0, i+2.0, i+3.0, i+4.0 }, "struct1::one"); 266 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 267 | bm_v.push_back({ (double)i, i*2.0, i*3.0, i*4.0 }, "struct1::two"); 268 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 269 | bm_v.push_back({ (double)i, i/2.0, i/3.0, i/4.0 }, "struct2::one"); 270 | } 271 | 272 | ``` 273 | 274 | ``` 275 | buffer_manager_test_nested_vector = 276 | 277 | struct with fields: 278 | 279 | description_list: {[1×0 char]} 280 | struct2: [1×1 struct] 281 | struct1: [1×1 struct] 282 | 283 | >> buffer_manager_test_nested_vector.struct1 284 | 285 | ans = 286 | 287 | struct with fields: 288 | 289 | two: [1×1 struct] 290 | one: [1×1 struct] 291 | 292 | >> buffer_manager_test_nested_vector.struct1.one 293 | 294 | ans = 295 | 296 | struct with fields: 297 | 298 | data: [4×1×3 double] 299 | dimensions: [4 1 3] 300 | name: 'one' 301 | timestamps: [1.6415e+09 1.6415e+09 1.6415e+09] 302 | ``` 303 | 304 | ### Example multiple types 305 | 306 | ``BufferManager`` can be used to store channels of different types, including ``struct``s. 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``](https://github.com/ami-iit/matio-cpp). 307 | ```c++ 308 | struct testStruct 309 | { 310 | int a; 311 | double b; 312 | }; 313 | VISITABLE_STRUCT(testStruct, a, b); 314 | 315 | ... 316 | 317 | robometry::BufferManager bm; 318 | robometry::BufferConfig bufferConfig; 319 | 320 | robometry::ChannelInfo var_int{ "int_channel", {1}}; 321 | robometry::ChannelInfo var_double{ "double_channel", {1}}; 322 | robometry::ChannelInfo var_string{ "string_channel", {1}}; 323 | robometry::ChannelInfo var_vector{ "vector_channel", {4, 1}}; 324 | robometry::ChannelInfo var_struct{ "struct_channel", {1}}; 325 | 326 | bm.addChannel(var_int); 327 | bm.addChannel(var_double); 328 | bm.addChannel(var_string); 329 | bm.addChannel(var_vector); 330 | bm.addChannel(var_struct); 331 | 332 | bufferConfig.n_samples = 3; 333 | bufferConfig.filename = "buffer_manager_test_multiple_types"; 334 | bufferConfig.auto_save = true; 335 | 336 | bm.configure(bufferConfig); 337 | 338 | testStruct item; 339 | 340 | for (int i = 0; i < 3; i++) { 341 | bm.push_back(i, "int_channel"); 342 | bm.push_back(i * 1.0, "double_channel"); 343 | bm.push_back("iter" + std::to_string(i), "string_channel"); 344 | bm.push_back({i + 0.0, i + 1.0, i + 2.0, i + 3.0}, "vector_channel"); 345 | item.a = i; 346 | item.b = i; 347 | bm.push_back(item, "struct_channel"); 348 | 349 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 350 | } 351 | } 352 | 353 | ``` 354 | The above snippet of code generates channels of different types. It produces the following output. 355 | ``` 356 | >> buffer_manager_test_multiple_types 357 | 358 | buffer_manager_test_multiple_types = 359 | 360 | struct with fields: 361 | 362 | description_list: {[1×0 char]} 363 | yarp_robot_name: [1×0 char] 364 | struct_channel: [1×1 struct] 365 | vector_channel: [1×1 struct] 366 | string_channel: [1×1 struct] 367 | double_channel: [1×1 struct] 368 | int_channel: [1×1 struct] 369 | 370 | >> buffer_manager_test_multiple_types.string_channel 371 | 372 | ans = 373 | 374 | struct with fields: 375 | 376 | data: {1×3 cell} 377 | dimensions: [1 3] 378 | elements_names: {'element_0'} 379 | units_of_measure: {'n.d.'} 380 | name: 'string_channel' 381 | timestamps: [1.6512e+09 1.6512e+09 1.6512e+09] 382 | 383 | >> buffer_manager_test_multiple_types.vector_channel 384 | 385 | ans = 386 | 387 | struct with fields: 388 | 389 | data: [4×1×3 double] 390 | dimensions: [4 1 3] 391 | elements_names: {4×1 cell} 392 | units_of_measure: {'n.d.'} 393 | name: 'vector_channel' 394 | timestamps: [1.6512e+09 1.6512e+09 1.6512e+09] 395 | 396 | ``` 397 | 398 | ### Example additional callback 399 | 400 | ``BufferManager`` can call an additional callback every time the save function is called. The 401 | following example define a custom callback that saves a dummy `txt` file along with the `mat` saved 402 | by the telemetry 403 | ```c++ 404 | bool myCallback(const std::string& file_name, const SaveCallbackSaveMethod& method) { 405 | std::string file_name_with_extension = file_name + ".txt"; 406 | std::ofstream my_file(file_name_with_extension.c_str()); 407 | 408 | // Write to the file 409 | my_file << "Dummy file!"; 410 | 411 | // Close the file 412 | my_file.close(); 413 | 414 | return true; 415 | }; 416 | 417 | 418 | robometry::BufferManager bm; 419 | bm.setSaveCallback(myCallback); 420 | ``` 421 | 422 | ### Example configuration file 423 | 424 | It is possible to load the configuration of a BufferManager **from a json file** 425 | ```c++ 426 | robometry::BufferManager bm; 427 | robometry::BufferConfig bufferConfig; 428 | bool ok = bufferConfigFromJson(bufferConfig,"test_json.json"); 429 | ok = ok && bm.configure(bufferConfig); 430 | ``` 431 | 432 | Where the file has to have this format: 433 | ```json 434 | { 435 | "yarp_robot_name": "robot", 436 | "description_list": [ 437 | "This is a test", 438 | "Or it isn't?" 439 | ], 440 | "path":"/my/preferred/path", 441 | "filename": "buffer_manager_test_conf_file", 442 | "n_samples": 20, 443 | "save_period": 1.0, 444 | "data_threshold": 10, 445 | "auto_save": true, 446 | "save_periodically": true, 447 | "channels": [ 448 | { 449 | "dimensions": [1,1], 450 | "elements_names": ["element_0"], 451 | "name": "one", 452 | "units_of_measure": ["meters"] 453 | }, 454 | { 455 | "dimensions": [1,1], 456 | "elements_names": ["element_0"], 457 | "name": "two", 458 | "units_of_measure": ["degrees"] 459 | } 460 | ], 461 | "enable_compression": true, 462 | "file_indexing": "%Y_%m_%d_%H_%M_%S", 463 | "mat_file_version": "v7_3" 464 | } 465 | ``` 466 | The configuration can be saved **to a json file** 467 | ```c++ 468 | robometry::BufferConfig bufferConfig; 469 | bufferConfig.n_samples = 10; 470 | bufferConfig.save_period = 0.1; //seconds 471 | bufferConfig.data_threshold = 5; 472 | bufferConfig.save_periodically = true; 473 | std::vector vars{ { "one",{2,3} }, 474 | { "two",{3,2} } }; 475 | bufferConfig.channels = vars; 476 | 477 | auto ok = bufferConfigToJson(bufferConfig, "test_json_write.json"); 478 | ``` 479 | 480 | 481 | ## TelemetryDeviceDumper 482 | 483 | 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. Moreover, if you would like to enable the flag `logIMotorTemperatures`, which will allow to collect motor temperatures in `motors_state::temperatures`, you need to use the yarp framework on [this branch](https://github.com/ami-iit/yarp/tree/yarp-3.10.1-motor-temperature), as explained in [this PR](https://github.com/robotology/yarp/pull/3188). 484 | 485 | >[!NOTE] 486 | Note that since robometry depends on `icub-main`, if you would like to enable the streaming of the motor temperatures, you need to also modify manually [these lines](https://github.com/robotology/icub-main/blob/master/CMakeLists.txt#L21-L22) in the `CMakeLists.txt` file in icub-main otherwise you will get a complain about the yarp minimum version used. Moreover, consider that, since this is still not merged on a stable branch, you might have compilation issues at the moment. 487 | 488 | ### Export the env variables 489 | * 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) ). 490 | * 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: 491 | ~~~ 492 | Yes, this is a YARP plugin 493 | * library: CMAKE_INSTALL_PREFIX/lib/yarp/yarp_telemetryDeviceDumper.dll 494 | * system version: 5 495 | * class name: robometry::TelemetryDeviceDumper 496 | * base class: yarp::dev::DeviceDriver 497 | ~~~ 498 | 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. 499 | 500 | Further documentation about the configuration parameters and the mapping of the variables inside the .mat file can be browsed [here](https://robotology.github.io/robometry/classrobometry_1_1TelemetryDeviceDumper.html) 501 | 502 | ## Contributing 503 | 504 | Pull requests are welcome. For major changes, please open an issue first to discuss what you would like to change. 505 | 506 | 507 | ## License 508 | [See License](https://github.com/robotology/robometry/blob/master/LICENSE) 509 | -------------------------------------------------------------------------------- /docs/Doxyfile-mcss.in: -------------------------------------------------------------------------------- 1 | @INCLUDE = Doxyfile-@ROBOMETRY_TAG@ 2 | 3 | USE_MDFILE_AS_MAINPAGE = "@ROBOMETRY_MAIN_PAGE@" 4 | 5 | 6 | GENERATE_HTML = NO 7 | GENERATE_XML = YES 8 | XML_PROGRAMLISTING = NO 9 | XML_NS_MEMB_FILE_SCOPE = YES 10 | 11 | XML_OUTPUT = "site/@ROBOMETRY_VERSION_PATH@/xml" 12 | HTML_OUTPUT = "site/@ROBOMETRY_VERSION_PATH@" 13 | 14 | PROJECT_BRIEF = "@ROBOMETRY_TAG@" 15 | -------------------------------------------------------------------------------- /docs/conf-master.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | DOXYFILE = 'Doxyfile-mcss-master' 4 | 5 | MAIN_PROJECT_URL = './index.html' 6 | SHOW_UNDOCUMENTED = True 7 | 8 | 9 | FINE_PRINT = """

robometry documentation. Licensed under the BSD 3-Clause "New" or "Revised" License. Project maintained by the iCub Tech department. Contact the team via GitHub.
Generated by Doxygen {doxygen_version} and m.css.""" 10 | 11 | 12 | versions = [('' + path + '', ) for path in next(os.walk('site'))[1] if path != 'xml'] 13 | 14 | versions.insert(0, ('Master', )) 15 | 16 | LINKS_NAVBAR1 = [ 17 | ('Pages', 'pages', []), 18 | ('Classes', 'annotated', []), 19 | ('Files', 'files', []), 20 | ] 21 | LINKS_NAVBAR2 = [ 22 | ('Versions', 'pages', versions), 23 | ("Contacts", 'pages', [ 24 | ("GitHub", ), 25 | ("Lab website", ), 26 | ]), 27 | ] 28 | -------------------------------------------------------------------------------- /docs/conf.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | DOXYFILE = 'Doxyfile-mcss-@ROBOMETRY_TAG@' 4 | 5 | MAIN_PROJECT_URL = '@ROBOMETRY_DOCUMENTATION_ROOT@/index.html' 6 | SHOW_UNDOCUMENTED = True 7 | 8 | 9 | FINE_PRINT = """

robometry documentation. Licensed under the BSD 3-Clause "New" or "Revised" License. Project maintained by the iCub Tech department. Contact the team via GitHub.
Generated by Doxygen {doxygen_version} and m.css.""" 10 | 11 | 12 | versions = [('' + path + '', ) for path in next(os.walk('site'))[1] if path != 'xml'] 13 | 14 | versions.insert(0, ('Master', )) 15 | 16 | LINKS_NAVBAR1 = [ 17 | ('Pages', 'pages', []), 18 | ('Classes', 'annotated', []), 19 | ('Files', 'files', []), 20 | ] 21 | LINKS_NAVBAR2 = [ 22 | ('Versions', 'pages', versions), 23 | ("Contacts", 'pages', [ 24 | ("GitHub", ), 25 | ("Lab website", ), 26 | ]), 27 | ] 28 | -------------------------------------------------------------------------------- /docs/config.toml: -------------------------------------------------------------------------------- 1 | [master] 2 | main_page = "README.md" 3 | additional_pages = ["docs/pages/magnitude_frequency_table.md", "CHANGELOG.md", "src/telemetryDeviceDumper/"] 4 | src_folder = "src" 5 | 6 | ['v1.1.0'] 7 | main_page = "README.md" 8 | additional_pages = ["docs/pages/magnitude_frequency_table.md", "CHANGELOG.md", "src/telemetryDeviceDumper/"] 9 | src_folder = "src" 10 | 11 | ['v1.0.0'] 12 | main_page = "README.md" 13 | additional_pages = ["docs/pages/magnitude_frequency_table.md", "CHANGELOG.md", "src/telemetryDeviceDumper/"] 14 | src_folder = "src" 15 | -------------------------------------------------------------------------------- /docs/generate_documentation_files.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) 4 | # This software may be modified and distributed under the terms of the 5 | # LGPL-2.1+ license. See the accompanying LICENSE file for details. 6 | 7 | import argparse 8 | import os 9 | 10 | 11 | def generate_documentation_files(tag: str, input_files_path: str, output_files_path: str, 12 | src_folder: str, main_page: str, additional_pages=[]): 13 | src_path = os.path.normpath(src_folder) 14 | input_files_path = os.path.normpath(input_files_path) 15 | output_files_path = os.path.normpath(output_files_path) 16 | main_page = os.path.normpath(main_page) 17 | _, main_page_extension = os.path.splitext(main_page) 18 | 19 | main_page_md = '' 20 | if main_page_extension == '.md': 21 | main_page_md = main_page 22 | 23 | folders = [os.path.normpath(folder[0]) for folder in os.walk(src_path) 24 | if os.path.basename(os.path.normpath(folder[0])) == 'include'] 25 | 26 | output_doxyfile_filename = 'Doxyfile-' + tag 27 | output_doxyfile_mcss_filename = 'Doxyfile-mcss-' + tag 28 | output_conf_filename = 'conf-' + tag + '.py' 29 | 30 | ROBOMETRY_VERSION_PATH = '.' 31 | if tag != 'master': 32 | ROBOMETRY_VERSION_PATH = tag 33 | 34 | ROBOMETRY_DOCUMENTATION_ROOT = '.' 35 | if tag != 'master': 36 | ROBOMETRY_DOCUMENTATION_ROOT = '..' 37 | 38 | try: 39 | input_file = open(os.path.join(input_files_path, 'Doxyfile.in'), 'rt') 40 | output_file = open(os.path.join(input_files_path, output_doxyfile_filename), 'wt') 41 | 42 | for line in input_file: 43 | output_file.write(line.replace('@ROBOMETRY_INCLUDE_FOLDERS@', 44 | "\"" + "\" \"".join(folders) + "\"") 45 | .replace('@ROBOMETRY_MAIN_PAGE@', main_page) 46 | .replace('@ROBOMETRY_ADDITIONAL_FILES@', "\"" + "\" \"".join(additional_pages) + "\"")) 47 | 48 | # Do something with the file 49 | except FileNotFoundError: 50 | print("Invalid doxyfile input file.") 51 | finally: 52 | input_file.close() 53 | output_file.close() 54 | 55 | try: 56 | input_file = open(os.path.join(input_files_path, 'Doxyfile-mcss.in'), 'rt') 57 | output_file = open(os.path.join(input_files_path, output_doxyfile_mcss_filename), 'wt') 58 | 59 | for line in input_file: 60 | output_file.write(line.replace('@ROBOMETRY_TAG@', tag).replace('@ROBOMETRY_VERSION_PATH@', ROBOMETRY_VERSION_PATH).replace( 61 | '@ROBOMETRY_MAIN_PAGE@', main_page_md)) 62 | 63 | # Do something with the file 64 | except FileNotFoundError: 65 | print("Invalid doxyfile-mcss input file.") 66 | finally: 67 | input_file.close() 68 | output_file.close() 69 | 70 | try: 71 | input_file = open(os.path.join(input_files_path, 'conf.py.in'), 'rt') 72 | output_file = open(os.path.join(input_files_path, output_conf_filename), 'wt') 73 | 74 | for line in input_file: 75 | output_file.write( 76 | line.replace('@ROBOMETRY_TAG@', tag).replace('@ROBOMETRY_DOCUMENTATION_ROOT@', ROBOMETRY_DOCUMENTATION_ROOT)) 77 | 78 | # Do something with the file 79 | except FileNotFoundError: 80 | print("Invalid conf.py input file.") 81 | finally: 82 | input_file.close() 83 | output_file.close() 84 | 85 | 86 | if __name__ == '__main__': 87 | parser = argparse.ArgumentParser( 88 | description='generate_doxyfile.py is a python script useful to generate the Doxyfile from Doxyfile.in.') 89 | parser.add_argument('--tag', type=str, default='master', required=False, help='Version of project.') 90 | parser.add_argument('--input_files_path', type=str, default='./', required=False, help='Path of the input files.') 91 | parser.add_argument('--output_files_path', type=str, default='./', required=False, help='Path of the output files.') 92 | parser.add_argument('--src_folder', type=str, required=True, help='Path of the src folder.') 93 | parser.add_argument('--main_page', type=str, required=True, help='Path to the the main page.') 94 | args = parser.parse_args() 95 | 96 | generate_documentation_files(tag=args.tag, 97 | input_files_path=args.input_files_path, 98 | output_files_path=args.output_files_path, 99 | src_folder=args.src_folder, 100 | main_page=args.main_page) 101 | -------------------------------------------------------------------------------- /docs/generate_website.py: -------------------------------------------------------------------------------- 1 | #!/bin/python3 2 | 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # This software may be modified and distributed under the terms of the 5 | # LGPL-2.1+ license. See the accompanying LICENSE file for details. 6 | 7 | import toml 8 | import subprocess 9 | import os 10 | import argparse 11 | 12 | from generate_documentation_files import generate_documentation_files 13 | 14 | def generate_documentation(tag, options): 15 | print("###### Generating documentation for", tag, " ######") 16 | 17 | root_folder = "../" 18 | if tag != 'master': 19 | os.chdir('robometry') 20 | subprocess.Popen(["git", "checkout", tag], stdout=subprocess.PIPE) 21 | os.chdir('..') 22 | root_folder = "robometry/" 23 | 24 | additional_pages = [] 25 | if 'additional_pages' in options.keys(): 26 | additional_pages = [root_folder + page for page in options['additional_pages']] 27 | 28 | generate_documentation_files(tag=tag, 29 | src_folder=root_folder + options['src_folder'], 30 | main_page=root_folder + options['main_page'], 31 | additional_pages=additional_pages, 32 | input_files_path="./", 33 | output_files_path="./") 34 | 35 | try: 36 | subprocess.check_call(["python3", args.mcss_path, "conf-" + tag + ".py"]) 37 | except subprocess.CalledProcessError as error: 38 | raise ValueError(error) 39 | 40 | 41 | if __name__ == "__main__": 42 | parser = argparse.ArgumentParser(description='generate_website.py is a python script useful to generate the ' 43 | 'documentation of the robometry repo.') 44 | parser.add_argument('--mcss_path', 45 | type=str, 46 | required=True, 47 | help='Path to \'m.css/documentation/doxygen.py\' file') 48 | args = parser.parse_args() 49 | 50 | parameters = toml.load("config.toml") 51 | 52 | # create the site directory structure 53 | os.makedirs('./site', exist_ok=True) 54 | for key, value in parameters.items(): 55 | if key != 'master': 56 | directory = './site/' + key 57 | os.makedirs(directory, exist_ok=True) 58 | 59 | # build the documentation 60 | for key, value in parameters.items(): 61 | generate_documentation(key, value) 62 | 63 | -------------------------------------------------------------------------------- /docs/pages/magnitude_frequency_table.md: -------------------------------------------------------------------------------- 1 | In the following table we have a list of the frequencies and sizes of the data we are working with. This is important to calculate how much time we have between two consecutive readings, to test the performance of the system (how fast it can write to the circular buffer) 2 | 3 | | Data type | Unitary dimension | Occurrences | Frequency | Type | 4 | |:-------------------------------------:|:-----------------:|:-----------:|:---------:|----------------------------------------------------| 5 | | desired joint positions | 23x1 | 1 | 100Hz | ``double`` | 6 | | measured joint positions | 23x1 | 1 | 100Hz | ``double`` | 7 | | desired joint velocities | 23x1 | 1 | 100Hz | ``double`` | 8 | | measured joint velocities | 23x1 | 1 | 100Hz | ``double`` | 9 | | desired joint accelerations | 23x1 | 1 | 100Hz | ``double`` | 10 | | measured joint accelerations | 23x1 | 1 | 100Hz | ``double`` | 11 | | desired joint torques | 23x1 | 1 | 100Hz | ``double`` | 12 | | measured joint torques | 23x1 | 1 | 100Hz | ``double`` | 13 | | measured joint currents | 23x1 | 1 | 100Hz | ``double`` | 14 | | measured joint PWM | 23x1 | 1 | 100Hz | ``double`` | 15 | | desired CoM position | 3x1 | 1 | 100Hz | ``double`` | 16 | | measured CoM position | 3x1 | 1 | 100Hz | ``double`` | 17 | | desired momentum | 6x1 | 1 | 100Hz | ``double`` | 18 | | measured momentum | 6x1 | 1 | 100Hz | ``double`` | 19 | | estimated base transform | 4x4 | 1 | 100Hz | ``double`` | 20 | | estimated base velocity | 6x1 | 2 | 100Hz | ``double`` | 21 | | desired feet transform | 4x4 | 2 | 100Hz | ``double`` | 22 | | measured feet transform | 4x4 | 2 | 100Hz | ``double`` | 23 | | desired feet velocity | 6x1 | 2 | 100Hz | ``double`` | 24 | | measured feet velocity | 6x1 | 2 | 100Hz | ``double`` | 25 | | desired orientation of other frames | 4x4 | 4 | 100Hz | ``double`` | 26 | | desired contact wrenches | 6x1 | 4 | 100Hz | ``double`` | 27 | | measured contact wrenches | 6x1 | 4 | 100Hz | ``double`` | 28 | | IMU readings (orientation) | 3x1 | 5 | 100Hz | ``double`` | 29 | | IMU readings (acceleration) | 3x1 | 5 | 100Hz | ``double`` | 30 | | IMU readings (gyros) | 3x1 | 5 | 100Hz | ``double`` | 31 | | Mass matrix | 29x29 | 1 | 100Hz | ``double`` | 32 | | Corilis matrix | 29x1 | 1 | 100Hz | ``double`` | 33 | | Jacobians | 29x6 | 5 | 100Hz | ``double`` | 34 | | Messages coming from the robot boards | ? | 1 | ? | Text, separated by type (Error, Warning, Info, ..) | 35 | | controller settings | ? | 1 | Only once | Struct | 36 | 37 | Let's assume to have around 5ms of free time every 10ms. 38 | 39 | -------------------------------------------------------------------------------- /material/slides/app-logger.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/robometry/1ed898a14f1737f17e9fd7591021b06093e6f046/material/slides/app-logger.pptx -------------------------------------------------------------------------------- /material/slides/present-logging.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/robometry/1ed898a14f1737f17e9fd7591021b06093e6f046/material/slides/present-logging.pptx -------------------------------------------------------------------------------- /material/slides/telemetry_user_case.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/robometry/1ed898a14f1737f17e9fd7591021b06093e6f046/material/slides/telemetry_user_case.pptx -------------------------------------------------------------------------------- /material/slides/yarp-telemetry-20201014.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/robometry/1ed898a14f1737f17e9fd7591021b06093e6f046/material/slides/yarp-telemetry-20201014.pdf -------------------------------------------------------------------------------- /material/slides/yarp-telemetry-api.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/robometry/1ed898a14f1737f17e9fd7591021b06093e6f046/material/slides/yarp-telemetry-api.pdf -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | add_subdirectory(librobometry) 8 | if(YARP_os_FOUND AND YARP_dev_FOUND AND iCubDev_FOUND) 9 | add_subdirectory(telemetryDeviceDumper) 10 | endif() 11 | if(BUILD_EXAMPLES) 12 | add_subdirectory(examples) 13 | endif() 14 | -------------------------------------------------------------------------------- /src/examples/CB_to_matfile_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | using namespace std; 22 | using namespace robometry; 23 | 24 | std::mutex lock_mut; 25 | 26 | class storeData { 27 | 28 | private: 29 | bool closing; 30 | std::shared_ptr> cb; // shared pointer to circular buffer 31 | double period; 32 | double wait_interval; 33 | vector local_collection; // stores on the read-thread the values from the buffer 34 | 35 | public: 36 | 37 | // constructor of the read/save class. Initialized with the shared pointer and the read period 38 | storeData(std::shared_ptr> _cb, const double& _period) : cb(_cb), period(_period) 39 | { 40 | closing = false; 41 | } 42 | 43 | // destructor 44 | virtual ~storeData(){}; 45 | 46 | // function that periodically reads the buffer 47 | bool readBuffer() 48 | { 49 | while(!closing) 50 | { 51 | 52 | // we use yarp os Time to check how long it takes for next loop 53 | auto start = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); 54 | 55 | // we need to check if the buffer has actual data 56 | if(cb->empty()) 57 | { 58 | cout << "the buffer is empty! check the data receiver is still ok" << endl; 59 | wait_interval = period - (std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count() - start); 60 | cout << "Waiting for " << wait_interval << " seconds" << endl; 61 | if (wait_interval > 0) std::this_thread::sleep_for(std::chrono::milliseconds(int(wait_interval*1000))); 62 | continue; 63 | } 64 | 65 | // here we read and remove all elements. Each element we retrieve from the circular buffer should be removed (pop_back) to prevent reread 66 | lock_mut.lock(); 67 | for (long unsigned int i=0; i < cb->size(); i++) 68 | { 69 | // print the elements stored in the vector (for confirmation) 70 | auto castedDatum = any_cast>(cb->back().m_datum); 71 | for (auto f = castedDatum.begin(); f != castedDatum.end(); ++f) 72 | cout << *f << ' '; 73 | cout << endl; 74 | // store the elements into a local collection (independent of the circular buffer to allow reading more elements) 75 | local_collection.push_back(cb->back()); 76 | cout << "populated local collection" << endl; 77 | 78 | // clear the read entry 79 | cb->pop_back(); 80 | } 81 | lock_mut.unlock(); 82 | 83 | wait_interval = period - (std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count() - start); 84 | cout << "Waiting for " << wait_interval << " seconds" << endl; 85 | if (wait_interval > 0) std::this_thread::sleep_for(std::chrono::milliseconds(int(wait_interval*1000))); 86 | } 87 | cout << "stopping reading from buffer" << endl; 88 | return true; 89 | } 90 | 91 | // function that converts the data on Record to matioCpp elements, and saves everything to a mat file 92 | bool saveToFile() 93 | { 94 | 95 | // create copy of the local collection (this acts as a second buffer) 96 | lock_mut.lock(); 97 | vector _collection_copy = local_collection; 98 | lock_mut.unlock(); 99 | cout << "local copy created " << endl; 100 | 101 | vector linear_matrix; 102 | vector timestamp_vector; 103 | 104 | // the number of timesteps is the size of our collection 105 | auto num_timesteps = _collection_copy.size(); 106 | cout << "num timesteps: " << num_timesteps << endl; 107 | if(num_timesteps < 1) 108 | { 109 | cout << "not enough data points collected " << endl; 110 | return false; 111 | } 112 | // we assume the size of the data is the same for every timestep (is there any situation this would not be the case?) 113 | int size_datum = any_cast>(_collection_copy[0].m_datum).size(); 114 | cout << "size of datum: " << size_datum << endl; 115 | 116 | // we first collapse the matrix of data into a single vector, in preparation for matioCpp convertion 117 | cout << "linearizing matrix..." << endl; 118 | for (auto& _cell : _collection_copy) 119 | { 120 | for (auto& _el : any_cast>(_cell.m_datum)) 121 | { 122 | linear_matrix.push_back(_el); 123 | } 124 | timestamp_vector.push_back(_cell.m_ts); 125 | } 126 | cout << "matrix linearized " << endl; 127 | 128 | // now we start the matioCpp convertion process 129 | 130 | // first create timestamps vector 131 | matioCpp::Vector timestamps("timestamps"); 132 | timestamps = timestamp_vector; 133 | 134 | // and the structures for the actual data too 135 | vector test_data; 136 | 137 | // now we create the vector for the dimensions 138 | vector dimensions_data_vect{(int)num_timesteps, size_datum}; 139 | matioCpp::Vector dimensions_data("dimensions"); 140 | dimensions_data = dimensions_data_vect; 141 | 142 | // now we populate the matioCpp matrix 143 | matioCpp::MultiDimensionalArray out("test", {(unsigned long int)size_datum, (unsigned long int)num_timesteps}, linear_matrix.data()); 144 | test_data.emplace_back(out); // Data 145 | 146 | test_data.emplace_back(dimensions_data); // dimensions vector 147 | 148 | test_data.emplace_back(matioCpp::String("name", "test data")); // name of the signal 149 | 150 | // we store it as a matioCpp struct 151 | matioCpp::Struct data_struct("test_data", test_data); 152 | 153 | // now we create the vector that stores different signals (in case we had more than one) 154 | vector signalsVect; 155 | signalsVect.emplace_back(data_struct); 156 | 157 | // and the matioCpp struct for these signals 158 | matioCpp::Struct signals("signals", signalsVect); 159 | 160 | // now we initialize the proto-timeseries structure 161 | vector timeSeries_data; 162 | 163 | // the timestamps vector is stored in parallel to the signals 164 | timeSeries_data.emplace_back(timestamps); 165 | timeSeries_data.emplace_back(signals); 166 | 167 | matioCpp::Struct timeSeries("Output", timeSeries_data); 168 | 169 | // and finally we write the file 170 | matioCpp::File file = matioCpp::File::Create("test_cb.mat"); 171 | file.write(timeSeries); 172 | 173 | return true; 174 | } 175 | 176 | bool close() 177 | { 178 | return closing = true; 179 | } 180 | }; 181 | 182 | 183 | int main() 184 | { 185 | 186 | /* generate random integer vector with 10 entries */ 187 | random_device rnd_device; 188 | mt19937 mersenne_engine {rnd_device()}; // Generates random integers 189 | uniform_int_distribution dist {1, 52}; 190 | auto gen = [&dist, &mersenne_engine](){ 191 | return dist(mersenne_engine); 192 | }; 193 | 194 | 195 | string input_comm = ""; 196 | vector vec(10); 197 | /**************************************************/ 198 | 199 | // Initialization of our Buffer (3 entries, type vector) 200 | Buffer cb(3); 201 | double period = 5; // period for the reading of the buffer 202 | 203 | // Initialization of our reading and saving to file class - uses the shared-pointer for reading the circular buffer 204 | storeData storer(cb.getBufferSharedPtr(), period); 205 | 206 | // only the reading function will be ran in a separate thread 207 | thread my_thread(&storeData::readBuffer, std::ref(storer)); 208 | 209 | // loop that populates the circular buffer. write "no" when prompted in order to save the stored data to a mat file 210 | while(true) 211 | { 212 | generate(begin(vec), end(vec), gen); // generates the random vector 213 | 214 | // we lock before we populate the circular buffer to prevent conflicts with reading 215 | lock_mut.lock(); 216 | cb.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vec})); 217 | lock_mut.unlock(); 218 | 219 | // user input -> say "no" to close the loop and generate the mat file 220 | cout << "shall we continue?" << endl; 221 | cin >> input_comm; 222 | cout << input_comm << endl; 223 | if(input_comm.find("no") != std::string::npos) 224 | break; 225 | } 226 | 227 | storer.close(); // close the reading loop 228 | std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 229 | cout << "joining thread..." << endl; 230 | my_thread.join(); // terminate thread 231 | cout << "saving to file" << endl; 232 | storer.saveToFile(); // save to mat file 233 | cout << "closing" << endl; 234 | return 0; 235 | } 236 | 237 | -------------------------------------------------------------------------------- /src/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | set(CIRCULAR_BUFFER_EXAMPLE_SRC circular_buffer_example.cpp) 8 | set(CIRCULAR_BUFFER_RECORD_EXAMPLE_SRC circular_buffer_record_example.cpp) 9 | set(MATIO_VECTOR_EXAMPLE_SRC matio_vector_example.cpp) 10 | set(MATIO_MATRIX_EXAMPLE_SRC matio_matrix_example.cpp) 11 | set(MATIO_TIMESERIES_EXAMPLE_SRC matio_timeseries_example.cpp) 12 | set(ROBOMETRY_BUFFER_EXAMPLE_SRC robometry_buffer_example.cpp) 13 | set(ROBOMETRY_BUFFER_MANAGER_CONF_FILE robometry_buffer_manager_conf_file.cpp) 14 | set(ROBOMETRY_BUFFER_MANAGER_EXAMPLE_SRC robometry_buffer_manager_example.cpp) 15 | set(ROBOMETRY_BUFFER_PERIODIC_SAVE_SRC robometry_buffer_periodic_save.cpp) 16 | set(CB_TO_MATIO_EXAMPLE_SRC CB_to_matfile_example.cpp) 17 | 18 | 19 | add_executable(circular_buffer_example ${CIRCULAR_BUFFER_EXAMPLE_SRC}) 20 | add_executable(circular_buffer_record_example ${CIRCULAR_BUFFER_RECORD_EXAMPLE_SRC}) 21 | add_executable(matio_vector_example ${MATIO_VECTOR_EXAMPLE_SRC}) 22 | add_executable(matio_matrix_example ${MATIO_MATRIX_EXAMPLE_SRC}) 23 | add_executable(matio_timeseries_example ${MATIO_TIMESERIES_EXAMPLE_SRC}) 24 | add_executable(robometry_buffer_example ${ROBOMETRY_BUFFER_EXAMPLE_SRC}) 25 | add_executable(robometry_buffer_manager_conf_file_example ${ROBOMETRY_BUFFER_MANAGER_CONF_FILE}) 26 | add_executable(robometry_buffer_manager_example ${ROBOMETRY_BUFFER_MANAGER_EXAMPLE_SRC}) 27 | add_executable(robometry_buffer_periodic_save ${ROBOMETRY_BUFFER_PERIODIC_SAVE_SRC}) 28 | add_executable(CB_to_matfile_example ${CB_TO_MATIO_EXAMPLE_SRC}) 29 | 30 | target_compile_features(circular_buffer_example PUBLIC cxx_std_17) 31 | target_compile_features(circular_buffer_record_example PUBLIC cxx_std_17) 32 | target_compile_features(robometry_buffer_example PUBLIC cxx_std_17) 33 | target_compile_features(robometry_buffer_manager_conf_file_example PUBLIC cxx_std_17) 34 | target_compile_features(robometry_buffer_manager_example PUBLIC cxx_std_17) 35 | target_compile_features(robometry_buffer_periodic_save PUBLIC cxx_std_17) 36 | 37 | 38 | target_link_libraries(circular_buffer_example Boost::boost) 39 | target_link_libraries(circular_buffer_record_example Boost::boost 40 | robometry::robometry) 41 | target_link_libraries(robometry_buffer_example robometry::robometry) 42 | target_link_libraries(robometry_buffer_manager_conf_file_example robometry::robometry) 43 | target_link_libraries(robometry_buffer_manager_example robometry::robometry) 44 | target_link_libraries(robometry_buffer_periodic_save robometry::robometry) 45 | target_link_libraries(matio_vector_example PRIVATE matioCpp::matioCpp) 46 | target_link_libraries(matio_matrix_example PRIVATE matioCpp::matioCpp) 47 | target_link_libraries(matio_timeseries_example PRIVATE matioCpp::matioCpp) 48 | target_link_libraries(CB_to_matfile_example PRIVATE matioCpp::matioCpp 49 | Boost::boost 50 | robometry::robometry 51 | ${CMAKE_THREAD_LIBS_INIT}) 52 | 53 | file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/conf/test_json.json 54 | DESTINATION ${CMAKE_BINARY_DIR}/bin) 55 | -------------------------------------------------------------------------------- /src/examples/circular_buffer_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | 17 | struct Data { 18 | std::chrono::time_point ts; 19 | int datum; 20 | Data(const std::chrono::time_point& _ts, 21 | const int& _datum) : ts(_ts), datum(_datum) { 22 | std::cout<<"Data(_ts, _datum)"< cb(3); 61 | 62 | // Insert threee elements into the buffer. 63 | cb.push_back(Data(std::chrono::system_clock::now(), 1)); 64 | std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 65 | cb.push_back(Data(std::chrono::system_clock::now(), 2)); 66 | std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 67 | cb.push_back(Data(std::chrono::system_clock::now(), 3)); 68 | std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 69 | 70 | cout<<"The circular buffer contains:"< 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | using namespace robometry; 21 | 22 | int main() 23 | { 24 | 25 | std::cout<<"XXXXXXXX CIRCULAR BUFFER OF INT XXXXXXXX"< cb_i(3); 28 | 29 | size_t total_payload = 0; 30 | cout<<"The capacity is: "<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 1 }})); 33 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 34 | cb_i.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 2 }})); 35 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 36 | cb_i.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 3 }})); 37 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 38 | 39 | cout<<"The capacity is: "<>(c_el.m_datum)[0]<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 4 }})); 46 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 47 | cb_i.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 5 }})); 48 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 49 | 50 | 51 | cout<<"The capacity is: "<>(c_el.m_datum)[0]< cb_d(3); 61 | 62 | cout<<"The capacity is: "<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.1 }})); 65 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 66 | cb_d.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.2 }})); 67 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 68 | cb_d.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.3 }})); 69 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 70 | 71 | cout<<"The capacity is: "<>(c_el.m_datum)[0]<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.4 }})); 78 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 79 | cb_d.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.5 }})); 80 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 81 | 82 | 83 | cout<<"The capacity is: "<>(c_el.m_datum)[0]< cb_v(3); 94 | 95 | cout<<"The capacity is: "<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{0.1, 0.2, 0.3}})); 98 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 99 | cb_v.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{0.3, 0.4, 0.5}})); 100 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 101 | cb_v.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{0.6, 0.7, 0.8}})); 102 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 103 | 104 | cout<<"The capacity is: "<>(c_el.m_datum)) 109 | { 110 | cout<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{0.9, 1.0, 1.1}})); 116 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 117 | cb_v.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{1.2, 1.3, 1.4}})); 118 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 119 | 120 | 121 | cout<<"The capacity is: "<>(c_el.m_datum)) 126 | { 127 | cout< 10 | #include 11 | 12 | using namespace std; 13 | 14 | int main(int argc, char *argv[]) 15 | { 16 | matioCpp::File file = matioCpp::File::Create("test_int.mat"); 17 | matioCpp::File file_double = matioCpp::File::Create("test_double.mat"); 18 | matioCpp::File file_3d = matioCpp::File::Create("test_3d.mat"); 19 | 20 | // Create a matio matrix of ints 2x2 (from vector) 21 | vector in = {2, 4, 7, 10}; 22 | matioCpp::MultiDimensionalArray out("test", {2,2}, in.data()); 23 | 24 | // Create a matio matrix of ints 2x2x2 (from vector) 25 | vector in_3d = {2, 4, 7, 9, 10, 13, 21, 24}; 26 | matioCpp::MultiDimensionalArray out_3d("test", {2,2,2}, in_3d.data()); 27 | 28 | // create a matio matrix of doubles 29 | vector in_double = {2.0, 4.0, 6.0, 8.0}; 30 | matioCpp::MultiDimensionalArray out_double("test", {2,2}, in_double.data()); 31 | 32 | // adding another 2x2 matrix to an existing matrix 33 | out_double.resize({2, 2, 2}); // clears previous data 34 | out_double({0,0,0}) = in_double[0]; 35 | out_double({0,1,0}) = in_double[1]; 36 | out_double({0,0,1}) = in_double[2]; 37 | out_double({0,1,1}) = in_double[3]; 38 | out_double({1,0,0}) = 40.0; 39 | out_double({1,1,0}) = 50.0; 40 | out_double({1,0,1}) = 60.0; 41 | out_double({1,1,1}) = 70.0; 42 | // {#vectors_vectors, #vectors, #element} 43 | 44 | file.write(out); 45 | file_double.write(out_double); 46 | file_3d.write(out_3d); 47 | 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /src/examples/matio_timeseries_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | int main(int argc, char *argv[]) 15 | { 16 | matioCpp::File file = matioCpp::File::Create("test_timeseries.mat"); 17 | 18 | 19 | // first we take care of the timestamps vector 20 | unsigned long int num_time_stamps = 3; 21 | vector timestamps_vect{0.5, 1.0, 1.5}; 22 | 23 | 24 | // convert from c++ vector to matioCpp vector 25 | matioCpp::Vector timestamps("timestamps"); 26 | timestamps = timestamps_vect; 27 | 28 | 29 | // now we initialize the proto-timeseries structure 30 | vector timeSeries_data; 31 | 32 | 33 | // and the structures for the actual data too 34 | vector data_left_arm; 35 | vector data_right_arm; 36 | 37 | 38 | // here we create the matioCpp vector for the dimensions of the joints vector (#steps, #joints) 39 | vector dimensions_left_vect{(int)num_time_stamps, 4}; 40 | matioCpp::Vector dimensions_left("dimensions"); 41 | dimensions_left = dimensions_left_vect; 42 | 43 | 44 | // now we start populating the proto-structure for the left arm data 45 | // we start by creating an empty vector with the dimensions specified 46 | // (In practice, we will be creating the matioCpp vector as we did for the dimensions) 47 | data_left_arm.emplace_back(matioCpp::MultiDimensionalArray("joints", {num_time_stamps,4})); 48 | 49 | // now we add also the dimensions vector to this proto-structure 50 | data_left_arm.emplace_back(dimensions_left); 51 | 52 | // we can also set the name of the signal inside this data 53 | data_left_arm.emplace_back(matioCpp::String("name", "left_arm")); 54 | 55 | // and finally we convert the proto-structure into an actual matioCpp Struct 56 | matioCpp::Struct left_arm_struct("left_arm", data_left_arm); 57 | 58 | 59 | // now we repeat the steps above but for the right arm (in practice just changing the dimensions) 60 | vector dimensions_right_vect{(int)num_time_stamps, 6}; 61 | matioCpp::Vector dimensions_right("dimensions"); 62 | dimensions_right = dimensions_right_vect; 63 | 64 | data_right_arm.emplace_back(matioCpp::MultiDimensionalArray("joints", {num_time_stamps,6})); 65 | data_right_arm.emplace_back(dimensions_right); 66 | data_right_arm.emplace_back(matioCpp::String("name", "right_arm")); 67 | matioCpp::Struct right_arm_struct("right_arm", data_right_arm); 68 | 69 | 70 | 71 | // now we create a vector with all the structs (signals) we have.... 72 | vector signalsVect; 73 | signalsVect.emplace_back(left_arm_struct); 74 | signalsVect.emplace_back(right_arm_struct); 75 | 76 | // ...and convert this vector into a matioCpp Struct 77 | matioCpp::Struct signals("signals", signalsVect); 78 | 79 | 80 | // Now, we populate the timeseries proto-struct with the timestamp vector (common for both signals) and the struct of signals 81 | timeSeries_data.emplace_back(timestamps); 82 | timeSeries_data.emplace_back(signals); 83 | 84 | 85 | // finally we convert the proto-structure into a matioCpp Struct, ready for writing 86 | matioCpp::Struct timeSeries("output", timeSeries_data); 87 | 88 | 89 | file.write(timeSeries); 90 | 91 | return 0; 92 | } 93 | 94 | -------------------------------------------------------------------------------- /src/examples/matio_vector_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | int main(int argc, char *argv[]) 15 | { 16 | matioCpp::File file = matioCpp::File::Create("test_int.mat"); 17 | matioCpp::File file_double = matioCpp::File::Create("test_double.mat"); 18 | 19 | // Create a matio vector of ints 20 | vector in = {2, 4, 7, 10}; 21 | matioCpp::Vector out("test"); 22 | out = in; //"in" is automatically converted to span! 23 | 24 | // create a matio vector of doubles 25 | vector in_double = {2.0, 4.0, 6.0, 8.0}; 26 | matioCpp::Vector out_double("test"); 27 | out_double = in_double; //"in" is automatically converted to span! 28 | 29 | file.write(out); 30 | file_double.write(out_double); 31 | 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /src/examples/robometry_buffer_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | using namespace robometry; 20 | 21 | int main() 22 | { 23 | 24 | std::cout<<"XXXXXXXX ROBOMETRY BUFFER OF INT XXXXXXXX"<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 1 }})); 30 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 31 | cb_i.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 2 }})); 32 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 33 | cb_i.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 3 }})); 34 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 35 | 36 | cout<<"The space available is: "<>(c_el.m_datum)[0]<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 4 }})); 43 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 44 | cb_i.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 5 }})); 45 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 46 | 47 | 48 | cout<<"The space available is: "<>(c_el.m_datum)[0]<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.1 }})); 62 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 63 | cb_d.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.2 }})); 64 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 65 | cb_d.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.3 }})); 66 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 67 | 68 | cout<<"The space available is: "<>(c_el.m_datum)[0]<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.4 }})); 75 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 76 | cb_d.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{ 0.5 }})); 77 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 78 | 79 | 80 | cout<<"The space available is: "<>(c_el.m_datum)[0]<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{0.1, 0.2, 0.3}})); 93 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 94 | cb_v.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{0.3, 0.4, 0.5}})); 95 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 96 | cb_v.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{0.6, 0.7, 0.8}})); 97 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 98 | 99 | cout<<"The space available is: "<>(c_el.m_datum)) 104 | { 105 | cout<(std::chrono::system_clock::now().time_since_epoch()).count(), vector{0.9, 1.0, 1.1}})); 111 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 112 | cb_v.push_back(Record({std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(), vector{1.2, 1.3, 1.4}})); 113 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 114 | 115 | 116 | cout<<"The space available is: "<>(c_el.m_datum)) 121 | { 122 | cout< 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | 21 | constexpr size_t n_samples{20}; 22 | constexpr size_t threshold{10}; 23 | constexpr double check_period{1.0}; 24 | constexpr auto file_indexing = "%Y_%m_%d_%H_%M_%S"; 25 | 26 | 27 | int main() 28 | { 29 | robometry::BufferConfig bufferConfig; 30 | 31 | // we configure our API to use our periodic saving option 32 | bufferConfig.filename = "test_json_write"; 33 | bufferConfig.n_samples = n_samples; 34 | bufferConfig.save_period = check_period; 35 | bufferConfig.data_threshold = threshold; 36 | bufferConfig.save_periodically = true; 37 | bufferConfig.file_indexing = file_indexing; 38 | bufferConfig.mat_file_version = matioCpp::FileVersion::MAT7_3; 39 | 40 | std::vector vars{ { "one",{2,3} }, 41 | { "two",{3,2} } }; 42 | bufferConfig.channels = vars; 43 | 44 | auto ok = bufferConfigToJson(bufferConfig, "test_json_write.json"); 45 | 46 | if (!ok) { 47 | std::cout << "Problems saving configuration to json" << std::endl; 48 | return 1; 49 | } 50 | 51 | robometry::BufferManager bm; 52 | 53 | ok = bufferConfigFromJson(bufferConfig,"test_json_write.json"); 54 | 55 | ok = ok && bm.configure(bufferConfig); 56 | 57 | if (!ok) { 58 | std::cout << "Problems configuring from file" << std::endl; 59 | return 1; 60 | } 61 | 62 | std::cout << "Starting loop" << std::endl; 63 | for (int i = 0; i < 40; i++) { 64 | bm.push_back({ i }, "one"); 65 | bm.push_back({ i + 1 }, "one"); 66 | bm.push_back(std::vector{ i + 2 }, "one"); 67 | } 68 | std::this_thread::sleep_for(std::chrono::seconds(3)); 69 | return 0; 70 | } 71 | -------------------------------------------------------------------------------- /src/examples/robometry_buffer_manager_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | constexpr size_t n_samples{3}; 21 | 22 | int main() 23 | { 24 | // The inputs to the API are defined in the BufferConfig structure 25 | robometry::BufferConfig bufferConfig; 26 | 27 | // We use the default config, setting only the number of samples (no auto/periodic saving) 28 | bufferConfig.n_samples = n_samples; 29 | 30 | robometry::BufferManager bm(bufferConfig); 31 | bm.setFileName("buffer_manager_test"); 32 | auto ok{false}; 33 | robometry::ChannelInfo var_one{ "one", {1,1} }; 34 | robometry::ChannelInfo var_two{ "two", {1,1} }; 35 | 36 | ok = bm.addChannel(var_one); 37 | ok = ok && bm.addChannel(var_two); 38 | if (!ok) { 39 | std::cout << "Problem adding variables...."< vars{ { "one",{2,3} }, 60 | { "two",{3,2} } }; 61 | 62 | ok = bm_m.addChannels(vars); 63 | if (!ok) { 64 | std::cout << "Problem adding variables...."< 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | constexpr size_t n_samples{20}; 21 | constexpr size_t threshold{10}; 22 | constexpr double check_period{1.0}; 23 | 24 | 25 | 26 | int main() 27 | { 28 | 29 | robometry::BufferConfig bufferConfig; 30 | 31 | // we configure our API to use our periodic saving option 32 | bufferConfig.n_samples = n_samples; 33 | bufferConfig.save_period = check_period; 34 | bufferConfig.data_threshold = threshold; 35 | bufferConfig.save_periodically = true; 36 | 37 | robometry::BufferManager bm(bufferConfig); 38 | 39 | std::cout << "First example: " << std::endl; 40 | 41 | bm.setFileName("buffer_manager_test"); 42 | robometry::ChannelInfo var_one{ "one", {1,1} }; 43 | robometry::ChannelInfo var_two{ "two", {1,1} }; 44 | 45 | auto ok = bm.addChannel(var_one); 46 | ok = ok && bm.addChannel(var_two); 47 | if (!ok) { 48 | std::cout << "Problem adding variables...."< vars{ { "one",{2,3} }, 66 | { "two",{3,2} } }; 67 | 68 | ok = bm_m.addChannels(vars); 69 | if (!ok) { 70 | std::cout << "Problem adding variables...."< 49 | $ 50 | ) 51 | target_compile_features(robometry PUBLIC cxx_std_17) 52 | 53 | target_link_libraries(robometry PUBLIC Boost::boost 54 | matioCpp::matioCpp 55 | Threads::Threads 56 | PRIVATE nlohmann_json::nlohmann_json) 57 | # Support using filesystem on GCC < 9.1, 58 | # see https://en.cppreference.com/w/cpp/filesystem#Notes 59 | if((CMAKE_CXX_COMPILER_ID STREQUAL "GNU") AND (CMAKE_CXX_COMPILER_VERSION VERSION_LESS 9.1)) 60 | target_link_libraries(robometry PUBLIC stdc++fs) 61 | endif() 62 | list(APPEND ROBOMETRY_PUBLIC_DEPS Boost 63 | matioCpp 64 | Threads) 65 | list(APPEND ROBOMETRY_PRIVATE_DEPS nlohmann_json) 66 | 67 | set_target_properties(robometry PROPERTIES DEFINE_SYMBOL ROBOMETRY_EXPORTS) 68 | 69 | set_property(TARGET robometry PROPERTY PUBLIC_HEADER ${ROBOMETRY_HDRS}) 70 | set_property(TARGET robometry PROPERTY PRIVATE_HEADER ${ROBOMETRY_IMPL_HDRS}) 71 | set_property(TARGET robometry PROPERTY VERSION ${${CMAKE_PROJECT_NAME}_VERSION}) 72 | set_property(TARGET robometry PROPERTY SOVERSION 0) 73 | set_property(TARGET robometry PROPERTY FOLDER "Libraries") 74 | 75 | install( 76 | TARGETS robometry 77 | EXPORT robometry 78 | RUNTIME 79 | DESTINATION "${CMAKE_INSTALL_BINDIR}" 80 | COMPONENT robometry 81 | LIBRARY 82 | DESTINATION "${CMAKE_INSTALL_LIBDIR}" 83 | COMPONENT robometry 84 | NAMELINK_COMPONENT robometry-dev 85 | ARCHIVE 86 | DESTINATION "${CMAKE_INSTALL_LIBDIR}" 87 | COMPONENT robometry-dev 88 | PUBLIC_HEADER 89 | DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/robometry" 90 | COMPONENT robometry-dev 91 | PRIVATE_HEADER 92 | DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/yarp/telemetry/impl" 93 | COMPONENT robometry-priv-dev 94 | ) 95 | 96 | set(ROBOMETRY_PUBLIC_DEPS ${ROBOMETRY_PUBLIC_DEPS} PARENT_SCOPE) 97 | set(ROBOMETRY_PRIVATE_DEPS ${ROBOMETRY_PRIVATE_DEPS} PARENT_SCOPE) 98 | 99 | include(InstallBasicPackageFiles) 100 | 101 | install_basic_package_files(robometry 102 | COMPATIBILITY AnyNewerVersion 103 | VERSION ${robometry_VERSION} 104 | DEPENDENCIES ${ROBOMETRY_PUBLIC_DEPS} 105 | PRIVATE_DEPENDENCIES ${ROBOMETRY_PRIVATE_DEPS} 106 | EXPORT_DESTINATION ${CMAKE_BINARY_DIR}/${PROJECT_NAME}) 107 | -------------------------------------------------------------------------------- /src/librobometry/include/robometry/Buffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #ifndef ROBOMETRY_BUFFER_H 10 | #define ROBOMETRY_BUFFER_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace robometry { 19 | 20 | /** 21 | * @brief A class to represent the buffer of robometry::Record. 22 | * 23 | */ 24 | class Buffer { 25 | public: 26 | 27 | using iterator = typename boost::circular_buffer::iterator; 28 | using const_iterator = typename boost::circular_buffer::const_iterator; 29 | 30 | Buffer() = default; 31 | 32 | /** 33 | * @brief Construct a new Buffer object copying from another Buffer 34 | * 35 | * @param[in] _other Buffer to be copied. 36 | */ 37 | Buffer(const Buffer& _other) = default; 38 | 39 | /** 40 | * @brief Construct a new Buffer object moving from another Buffer 41 | * 42 | * @param[in] _other Buffer to be moved. 43 | */ 44 | Buffer(Buffer&& _other) noexcept = default; 45 | 46 | /** 47 | * @brief Copy assignment operator. 48 | * 49 | * @param[in] _other Buffer to be copied. 50 | * @return Buffer& Resulting Buffer. 51 | */ 52 | Buffer& operator=(const Buffer& _other) = default; 53 | 54 | /** 55 | * @brief Move assignment operator. 56 | * 57 | * @param[in] _other Buffer to be moved. 58 | 59 | * @return Buffer& Resulting Buffer. 60 | */ 61 | Buffer& operator=(Buffer&& _other) noexcept = default; 62 | 63 | /** 64 | * @brief Destroy the Buffer object 65 | * 66 | */ 67 | virtual ~Buffer() = default; 68 | /** 69 | * @brief Construct a new Buffer object. 70 | * 71 | * @param[in] num_elements Number of elements of the Buffer to be constructed. 72 | */ 73 | Buffer(size_t num_elements); 74 | 75 | /** 76 | * @brief Push back copying the new Record. 77 | * 78 | * @param[in] elem Record to be copied 79 | */ 80 | void push_back(const Record &elem); 81 | 82 | /** 83 | * @brief Push back moving the new Record. 84 | * 85 | * @param[in] elem Record to be moved. 86 | */ 87 | void push_back(Record&& elem); 88 | 89 | /** 90 | * @brief Get the Buffer free space. 91 | * 92 | * @return size_t The free space expressed in bytes. 93 | */ 94 | size_t getBufferFreeSpace() const; 95 | 96 | /** 97 | * @brief Get the size of the Buffer. 98 | * 99 | * @return size_t The size of the buffer 100 | */ 101 | size_t size() const; 102 | 103 | /** 104 | * @brief Get the capacity of Buffer 105 | * 106 | * @return size_t The capacity of the buffer. 107 | */ 108 | size_t capacity() const; 109 | 110 | /** 111 | * @brief Return true if the Buffer is empty, false otherwise. 112 | 113 | * 114 | */ 115 | bool empty() const; 116 | 117 | /** 118 | * @brief Resize the Buffer. 119 | * 120 | * @param[in] new_size The new size to be resized to. 121 | */ 122 | void resize(size_t new_size); 123 | 124 | /** 125 | * @brief Change the capacity of the Buffer. 126 | * 127 | * @param[in] new_size The new size. 128 | */ 129 | void set_capacity(size_t new_size); 130 | 131 | /** 132 | * @brief Return true if the Buffer is full, false otherwise. 133 | * 134 | */ 135 | bool full() const; 136 | 137 | /** 138 | * @brief Return the iterator referred to the begin of the Buffer. 139 | * 140 | * @return iterator 141 | */ 142 | iterator begin() noexcept; 143 | 144 | /** 145 | * @brief Return the iterator referred to the end of the Buffer. 146 | * 147 | * @return iterator 148 | */ 149 | iterator end() noexcept; 150 | 151 | /** 152 | * @brief Return the const iterator referred to the begin of the Buffer. 153 | * 154 | * @return const_iterator 155 | */ 156 | const_iterator begin() const noexcept; 157 | 158 | /** 159 | * @brief Return the const iterator referred to the end of the Buffer. 160 | * 161 | * @return const_iterator 162 | */ 163 | const_iterator end() const noexcept; 164 | 165 | /** 166 | * @brief Clear the content of the buffer. 167 | * 168 | */ 169 | void clear() noexcept; 170 | 171 | /** 172 | * @brief Get the Buffer shared_ptr object. 173 | * 174 | * @return std::shared_ptr> 175 | */ 176 | std::shared_ptr> getBufferSharedPtr() const; 177 | 178 | private: 179 | std::shared_ptr> m_buffer_ptr; 180 | 181 | 182 | }; 183 | 184 | 185 | } // robometry 186 | 187 | #endif 188 | -------------------------------------------------------------------------------- /src/librobometry/include/robometry/BufferConfig.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #ifndef ROBOMETRY_BUFFER_CONFIG_H 10 | #define ROBOMETRY_BUFFER_CONFIG_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace robometry { 19 | using dimensions_t = std::vector; 20 | using elements_names_t = std::vector; 21 | using units_of_measure_t = std::vector; 22 | /** 23 | * @brief Struct representing a channel(variable) in terms of 24 | * name and dimensions and names of the each element of a variable. 25 | */ 26 | struct ChannelInfo { 27 | std::string name; /**< Name of the channel */ 28 | dimensions_t dimensions; /**< Dimension of the channel */ 29 | elements_names_t elements_names; /**< Vector containing the names of each element of the channel */ 30 | units_of_measure_t units_of_measure; /**< Units of measure of the channel */ 31 | /** 32 | * @brief Default constructor 33 | */ 34 | ChannelInfo() = default; 35 | /** 36 | *@brief Construct a ChannelInfo from name, dimensions, a vector containing the names 37 | of each element of the channel and the unit of measure of the channel. 38 | @param name name of the channel. 39 | @param dimensions dimensions of the channel. 40 | @param elements_names vector containing the names of each element of the channel. 41 | @param units_of_measure unit of measure of the channel. 42 | */ 43 | ChannelInfo(const std::string& name, 44 | const dimensions_t& dimensions, 45 | const elements_names_t& elements_names=elements_names_t(), 46 | const units_of_measure_t& units_of_measure=units_of_measure_t()); 47 | 48 | 49 | }; 50 | 51 | /** 52 | * @brief Struct containing the parameters for configuring a robometry::BufferManager. 53 | * 54 | */ 55 | struct BufferConfig { 56 | std::string yarp_robot_name{""}; /** < The yarp robot name associated to the machine where the logger runs */ 57 | std::vector description_list{""}; /** < the description list, e.g. it can contain the axes names that are logged*/ 58 | std::string path{ "" }; /**< the path in which the files will be saved. */ 59 | std::string filename{ "robometry_log" };/**< the file name, to it will be appended the suffix "_.mat". */ 60 | size_t n_samples{ 0 };/**< the max number of samples contained in the buffer/s */ 61 | double save_period{ 0.010 };/**< the period in sec of the save thread */ 62 | size_t data_threshold{ 0 };/**< the save thread saves to a file if there are at least data_threshold samples */ 63 | bool auto_save{ false };/**< the flag for enabling the save in the destructor of the robometry::BufferManager */ 64 | bool save_periodically{ false };/**< the flag for enabling the periodic save thread. */ 65 | std::vector channels;/**< the list of pairs representing the channels(variables) */ 66 | bool enable_compression{ false }; /**< the flag for enabling the zlib compression */ 67 | /** String representing the indexing mode. If the variable is set to `time_since_epoch`, `BufferManager::m_nowFunction` 68 | * is used. Othewrise `std::put_time` is used to generate the indexing. https://en.cppreference.com/w/cpp/io/manip/put_time */ 69 | std::string file_indexing{ "time_since_epoch" }; 70 | matioCpp::FileVersion mat_file_version{ matioCpp::FileVersion::Default }; /**< Version of the saved matfile. */ 71 | }; 72 | 73 | } // robometry 74 | 75 | /** 76 | * @brief Populate the robometry::BufferConfig struct reading from a json file. 77 | * 78 | * @param[out] bufferConfig The struct to be filled in. 79 | * @param[in] config_filename The name of the json file. 80 | * @return true on success, false otherwise. 81 | */ 82 | bool bufferConfigFromJson(robometry::BufferConfig& bufferConfig, const std::string& config_filename); 83 | 84 | /** 85 | * @brief Save on a json file the content of a robometry::BufferConfig struct. 86 | * 87 | * @param[in] bufferConfig The struct to be saved to file. 88 | * @param[in] config_filename The name of the json file to be saved. 89 | * @return true on success, false otherwise. 90 | */ 91 | bool bufferConfigToJson(const robometry::BufferConfig& bufferConfig, const std::string& config_filename); 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /src/librobometry/include/robometry/BufferManager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #ifndef ROBOMETRY_BUFFER_MANAGER_H 10 | #define ROBOMETRY_BUFFER_MANAGER_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #ifndef ROBOMETRY_UNUSED 37 | # define ROBOMETRY_UNUSED(x) (void)x; 38 | #endif // ROBOMETRY_UNUSED 39 | 40 | #ifndef __has_include 41 | static_assert(false, "__has_include not supported"); 42 | #else 43 | # if __has_include() 44 | # include 45 | namespace robometry_fs = std::filesystem; 46 | # elif __has_include() 47 | # include 48 | namespace robometry_fs = std::experimental::filesystem; 49 | # else 50 | static_assert(false, "Neither nor headers are present in the system, but they are required"); 51 | # endif 52 | #endif 53 | 54 | 55 | namespace robometry { 56 | 57 | /** 58 | * @brief Get the type name as string 59 | */ 60 | template 61 | static std::string getTypeName(const T& someInput) 62 | { 63 | return boost::core::demangle(typeid(someInput).name()); 64 | } 65 | 66 | /** 67 | * @brief Get the type name as string 68 | */ 69 | template 70 | static std::string getTypeName() 71 | { 72 | return boost::core::demangle(typeid(T).name()); 73 | } 74 | 75 | // matiomatioCppCanConcatenate::value is true when T has the T::value_type member. If this is true, then we check 76 | // if T is either an Element, a Vector (but not a String), or a MultidimensionalArray 77 | template 78 | struct matioCppCanConcatenate : std::false_type {}; 79 | 80 | template 81 | struct matioCppCanConcatenate::value>, 83 | typename std::enable_if_t<(std::is_same_v> || 84 | (std::is_same_v> && 85 | !std::is_same_v) || 86 | std::is_same_v>)>> 87 | : std::true_type {}; 88 | 89 | 90 | 91 | /** 92 | * @brief Class that aggregates the robometry::Buffer and some other 93 | * info(e.g. dimensions) used by the robometry::BufferManager 94 | * 95 | */ 96 | struct BufferInfo { 97 | inline static std::string type_name_not_set_tag = "type_name_not_set"; 98 | Buffer m_buffer; 99 | std::mutex m_buff_mutex; 100 | dimensions_t m_dimensions; 101 | size_t m_dimensions_factorial{0}; 102 | std::string m_type_name{type_name_not_set_tag}; 103 | elements_names_t m_elements_names; 104 | std::function m_convert_to_matioCpp; 105 | units_of_measure_t m_units_of_measure; 106 | 107 | BufferInfo() = default; 108 | BufferInfo(const BufferInfo& other) = default; 109 | 110 | BufferInfo(BufferInfo&& other) = default; 111 | 112 | // This method fills the m_convert_to_matioCpp lambda with a function able to convert the Buffer 113 | // into a matioCpp variable. This method is called when pushing the first time to a channel, 114 | // exploiting the fact that the push_back method is a template method 115 | // (and thus it is clear the type of input, necessary when casting std::any). 116 | template 117 | void createMatioCppConvertFunction() 118 | { 119 | // First check if we can use the matioCpp::make_variable function with the input type T. 120 | // If not, the input type is not compatible with matioCpp 121 | static_assert(matioCpp::is_make_variable_callable::value, "The selected type cannot be used with matioCpp."); 122 | 123 | // The lambda is generated only the first time that we push to a channel. 124 | // We are enforcing that the type pushed with push_back is always the same. 125 | if (m_convert_to_matioCpp) 126 | { 127 | return; 128 | } 129 | 130 | // The matioCpp::make_variable_output metafunction provides the type that would be output by matioCpp::make_variable 131 | using matioCppType = typename matioCpp::make_variable_output::type; 132 | 133 | // Start filling the m_convert_to_matioCpp lambda. The lambda will take as input the desired name and will output a matioCpp::Variable. 134 | m_convert_to_matioCpp = [this](const std::string& name) 135 | { 136 | size_t num_instants = this->m_buffer.size(); 137 | 138 | //--- 139 | //SCALAR CASE 140 | //if the input data is numeric (scalar, vector, multi-dimensional array), then we concatenate on the last dimension 141 | if constexpr (matioCppCanConcatenate::value) 142 | { 143 | //the scalar types in matioCpp have the member T::value_type, that is the type of each single element. 144 | using elementType = typename matioCppType::value_type; 145 | 146 | dimensions_t fullDimensions = this->m_dimensions; 147 | fullDimensions.push_back(num_instants); 148 | 149 | // The output is a multi dimensional array of dimensions n+1, where the last dimension is the number of time instants. 150 | matioCpp::MultiDimensionalArray outputVariable(name, fullDimensions); 151 | 152 | size_t t = 0; 153 | for (auto& _cell : this->m_buffer) { 154 | //We convert std::any type using the input type T. 155 | const T& cellCasted = std::any_cast(_cell.m_datum); 156 | 157 | matioCpp::Span matioCppSpan; 158 | 159 | matioCppType matioCppVariable; 160 | 161 | //We convert the cell to a matioCpp variable only if we are not able to create a Span (for example in case of scalars). 162 | //In this way we avoid duplicating memory 163 | if constexpr (matioCpp::SpanUtils::is_make_span_callable::value) 164 | { 165 | matioCppSpan = matioCpp::make_span(cellCasted); 166 | } 167 | else 168 | { 169 | matioCppVariable = matioCpp::make_variable("element", cellCasted); 170 | 171 | matioCppSpan = matioCppVariable.toSpan(); 172 | } 173 | 174 | size_t startIndex = this->m_dimensions_factorial * t; //We concatenate on the last dimension. Suppose that the channel stores matrices of size 3x2. 175 | //The output variable is a 3x2xn matrix, where n is the number of elements in the buffer. 176 | //If we consider the output buffer as a linear vector, the element at time t would start 177 | //from location 6*t and end at 6*(t+1) 178 | 179 | //matioCppSpan.size() should be equal to m_dimensions_factorial, but we avoid to perform this check for each input. 180 | //Hence, with std::min we make sure to avoid reading or wrinting in wrong pieces of memory 181 | for (size_t i = 0; i < std::min(static_cast(matioCppSpan.size()), this->m_dimensions_factorial); ++i) 182 | { 183 | outputVariable[startIndex + i] = matioCppSpan[i]; //we copy the new element in the corresponding position inside the variable 184 | } 185 | ++t; 186 | } 187 | return outputVariable; 188 | } 189 | 190 | //--- 191 | //STRUCT CASE 192 | else if constexpr(std::is_same_v) //if the input is a struct, we use a struct array 193 | { 194 | //The output variable would be a struct array of dimensions t. 195 | matioCpp::StructArray outputVariable(name, {1,num_instants}); 196 | 197 | size_t i = 0; 198 | for (auto& _cell : this->m_buffer) { 199 | matioCpp::Struct element = matioCpp::make_variable("element", std::any_cast(_cell.m_datum)); 200 | 201 | if (i == 0) 202 | { 203 | outputVariable.addFields(element.fields()); //Just for the first element, we specify the set of fields in the array 204 | } 205 | 206 | outputVariable.setElement(i, element); 207 | ++i; 208 | } 209 | return outputVariable; 210 | } 211 | 212 | //--- 213 | //CELL CASE (default) 214 | else //otherwise we use a cell array 215 | { 216 | //The output variable would be a cell array of dimensions t. 217 | matioCpp::CellArray outputVariable(name, {1,num_instants}); 218 | 219 | size_t i = 0; 220 | for (auto& _cell : this->m_buffer) { 221 | outputVariable.setElement(i, matioCpp::make_variable("element", std::any_cast(_cell.m_datum))); 222 | ++i; 223 | } 224 | return outputVariable; 225 | } 226 | }; 227 | } 228 | 229 | 230 | }; 231 | 232 | /** 233 | * @brief The SaveCallback may need to know if it is called in a periodic fashion or is the 234 | * last call before deallocating the class 235 | * 236 | */ 237 | enum class SaveCallbackSaveMethod { 238 | periodic, last_call 239 | }; 240 | 241 | /** 242 | * @brief Class that manages the buffers associated to the channels of the telemetry. 243 | * Each BufferManager can handle different types of data, the number of samples is defined in the configuration and 244 | * it is the same for every channel. 245 | * On the other hand the data inside the channels can have different dimensionality(e.g. 1x1, 2x3 etc). 246 | * It contains utilities for saving the data of the channels in mat files, and to save/read the configuration 247 | * to/from a json file. 248 | * 249 | */ 250 | class BufferManager { 251 | 252 | public: 253 | /** 254 | * @brief Construct an empty BufferManager object. 255 | * For being used it has to be configured afterwards. 256 | * 257 | */ 258 | BufferManager(); 259 | 260 | /** 261 | * @brief Construct a new BufferManager object, configuring it via 262 | * the robometry::BufferConfig. 263 | * 264 | * @param[in] _bufferConfig The struct containing the configuration for the BufferManager. 265 | */ 266 | BufferManager(const BufferConfig& _bufferConfig); 267 | 268 | /** 269 | * @brief Destroy the BufferManager object. 270 | * If auto_save is enabled, it saves to file the remaining data in the buffer. 271 | * 272 | */ 273 | ~BufferManager(); 274 | 275 | /** 276 | * @brief Enable the save thread with _save_period seconds of period. 277 | * If the thread has been started yet in the configuration through 278 | * BufferConfing, it skips it. 279 | * 280 | * @param[in] _save_period The period in seconds of the save thread. 281 | * @return true on success, false otherwise. 282 | */ 283 | bool enablePeriodicSave(double _save_period); 284 | 285 | /** 286 | * @brief Configure the BufferManager through a BufferConfig object. 287 | * 288 | * @param[in] _bufferConfig The struct containing the configuration parameters. 289 | * @return true on success, false otherwise. 290 | */ 291 | bool configure(const BufferConfig& _bufferConfig); 292 | 293 | /** 294 | * @brief Get the BufferConfig object representing the actual configuration. 295 | * 296 | * @return The BufferConfig object. 297 | */ 298 | BufferConfig getBufferConfig() const; 299 | /** 300 | * @brief Set the file name that will be created by the BufferManager. 301 | * 302 | * @param[in] filename The file name to be set. 303 | */ 304 | void setFileName(const std::string& filename); 305 | 306 | /** 307 | * @brief Set the path where the files will be saved. 308 | * 309 | * @param[in] path The path to be set. 310 | */ 311 | void setDefaultPath(const std::string& path); 312 | 313 | /** 314 | * @brief Enable the zlib compression. 315 | * 316 | * @param[in] flag for enabling/disabling compression. 317 | */ 318 | void enableCompression(bool enable_compression); 319 | 320 | /** 321 | * @brief Set the description list that will be saved in all the files. 322 | * 323 | * @param[in] description The description to be set. 324 | */ 325 | void setDescriptionList(const std::vector& description_list); 326 | 327 | /** 328 | * @brief Resize the Buffer/s. 329 | * 330 | * @param[in] new_size The new size to be resized to. 331 | */ 332 | void resize(size_t new_size); 333 | 334 | /** 335 | * @brief Set the capacity of Buffer/s. 336 | * 337 | * @param[in] new_size The new size. 338 | */ 339 | void set_capacity(size_t new_size); 340 | 341 | /** 342 | * @brief Add a channel(variable) to the BufferManager. 343 | * The channels have to be unique in the BufferManager. 344 | * 345 | * @param[in] channel Pair representing the channel to be added. 346 | * @return true on success, false otherwise. 347 | */ 348 | bool addChannel(const ChannelInfo& channel); 349 | 350 | /** 351 | * @brief Add a list of channels(variables) to the BufferManager. 352 | * The channels have to be unique in the BufferManager. 353 | * 354 | * @param[in] channels List of pair representing the channels to be added. 355 | * @return true on success, false otherwise. 356 | */ 357 | bool addChannels(const std::vector& channels); 358 | 359 | /** 360 | * @brief Push a new element in the var_name channel. 361 | * The var_name channels must exist, otherwise an exception is thrown. 362 | * 363 | * @param[in] elem The element to be pushed(via copy) in the channel. 364 | * @param[in] ts The timestamp of the element to be pushed. 365 | * @param[in] var_name The name of the channel. 366 | */ 367 | template 368 | inline void push_back(matioCpp::Span elem, double ts, const std::string& var_name) 369 | { 370 | push_back(std::vector(elem.begin(), elem.end()), ts, var_name); 371 | } 372 | 373 | /** 374 | * @brief Push a new element in the var_name channel. 375 | * The var_name channels must exist, otherwise an exception is thrown. 376 | * 377 | * @param[in] elem The element to be pushed(via copy) in the channel. 378 | * @param[in] ts The timestamp of the element to be pushed. 379 | * @param[in] var_name The name of the channel. 380 | */ 381 | template 382 | inline void push_back(const std::initializer_list& elem, double ts, const std::string& var_name) 383 | { 384 | push_back(std::vector(elem.begin(), elem.end()), ts, var_name); 385 | } 386 | 387 | /** 388 | * @brief Push a new element in the var_name channel. 389 | * The var_name channels must exist, otherwise an exception is thrown. 390 | * 391 | * @param[in] elem The element to be pushed(via copy) in the channel. 392 | * @param[in] var_name The name of the channel. 393 | */ 394 | template 395 | inline void push_back(matioCpp::Span elem, const std::string& var_name) 396 | { 397 | push_back(elem, m_nowFunction(), var_name); 398 | } 399 | 400 | /** 401 | * @brief Push a new element in the var_name channel. 402 | * The var_name channels must exist, otherwise an exception is thrown. 403 | * 404 | * @param[in] elem The element to be pushed(via copy) in the channel. 405 | * @param[in] var_name The name of the channel. 406 | */ 407 | template 408 | inline void push_back(const std::initializer_list& elem, const std::string& var_name) 409 | { 410 | push_back(elem, m_nowFunction(), var_name); 411 | } 412 | 413 | /** 414 | * @brief Push a new element in the var_name channel. 415 | * The var_name channels must exist, otherwise an exception is thrown. 416 | * 417 | * @param[in] elem The element to be pushed in the channel. 418 | * @param[in] ts The timestamp of the element to be pushed. 419 | * @param[in] var_name The name of the channel. 420 | */ 421 | template 422 | inline void push_back(const T& elem, double ts, const std::string& var_name) 423 | { 424 | auto leaf = getLeaf(var_name, m_tree).lock(); 425 | if (leaf == nullptr) 426 | { 427 | throw std::invalid_argument("The channel " + var_name + " does not exist."); 428 | } 429 | auto bufferInfo = leaf->getValue(); 430 | assert(bufferInfo != nullptr); 431 | 432 | bool typename_set = bufferInfo->m_type_name != BufferInfo::type_name_not_set_tag; 433 | 434 | if (typename_set && (bufferInfo->m_type_name != getTypeName())) 435 | { 436 | std::cout << "Cannot push to the channel " << var_name 437 | << ". Expected type: " << bufferInfo->m_type_name 438 | << ". Input type: " << getTypeName() < lock{ bufferInfo->m_buff_mutex }; 443 | 444 | if (!typename_set) 445 | { 446 | bufferInfo->m_type_name = getTypeName(); 447 | } 448 | 449 | //Create the saving functions if they were not present already 450 | bufferInfo->template createMatioCppConvertFunction(); 451 | 452 | bufferInfo->m_buffer.push_back({ts, elem}); 453 | } 454 | 455 | /** 456 | * @brief Push a new element in the var_name channel. 457 | * The var_name channels must exist, otherwise an exception is thrown. 458 | * 459 | * @param[in] elem The element to be pushed in the channel. 460 | * @param[in] var_name The name of the channel. 461 | */ 462 | template 463 | inline void push_back(const T& elem, const std::string& var_name) 464 | { 465 | push_back(elem, m_nowFunction(), var_name); 466 | } 467 | 468 | 469 | /** 470 | * @brief Save the content of all the channels into a file. 471 | * If flush_all is set to false, it saves only the content of the channels that 472 | * have a number of samples greater than the robometry::BufferConfig::data_threshold. 473 | * If robometry::BufferConfig::data_threshold is greater than robometry::BufferConfig::n_samples 474 | * this check is skipped. 475 | * 476 | * @param[in] flush_all Flag for forcing the save of whatever is contained in the channels. 477 | * @return true on success, false otherwise. 478 | */ 479 | bool saveToFile(bool flush_all=true); 480 | 481 | /** 482 | * @brief Save the content of all the channels into a file. 483 | * If flush_all is set to false, it saves only the content of the channels that 484 | * have a number of samples greater than the robometry::BufferConfig::data_threshold. 485 | * If robometry::BufferConfig::data_threshold is greater than robometry::BufferConfig::n_samples 486 | * this check is skipped. 487 | * 488 | * @param[in] flush_all Flag for forcing the save of whatever is contained in the channels. 489 | * @param[out] file_name_path path name of the matfile without the suffix .mat 490 | * @return true on success, false otherwise. 491 | */ 492 | bool saveToFile(std::string& file_name_path, bool flush_all=true); 493 | 494 | /** 495 | * @brief Set the now function, by default is std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(). 496 | * @param[in] now The now function 497 | * @return true on success, false otherwise. 498 | */ 499 | bool setNowFunction(std::function now); 500 | 501 | 502 | /** 503 | * @brief Set the saveCallback function. Thanks to this function you can save additional data 504 | * type along with the matfile salve by telemetry 505 | * @param[in] saveCallback The saveCallback function 506 | * @return true on success, false otherwise. 507 | */ 508 | bool setSaveCallback(std::function saveCallback); 509 | 510 | 511 | private: 512 | static double DefaultClock(); 513 | 514 | void periodicSave(); 515 | 516 | matioCpp::Struct createTreeStruct(const std::string& node_name, 517 | std::shared_ptr> tree_node, 518 | bool flush_all); 519 | 520 | matioCpp::Struct createElementStruct(const std::string& var_name, 521 | std::shared_ptr buffInfo, 522 | bool flush_all) const; 523 | 524 | /** 525 | * This is an helper function that can be used to generate the file indexing accordingly to the 526 | * content of `m_bufferConfig.file_indexing` 527 | * @return a string containing the index 528 | */ 529 | std::string fileIndex() const; 530 | 531 | /** 532 | * This is an helper function that will be disappear the day matio-cpp 533 | * will support the std::vector 534 | */ 535 | void populateDescriptionCellArray(); 536 | 537 | void resize(size_t new_size, std::shared_ptr> node); 538 | 539 | void set_capacity(size_t new_size, std::shared_ptr> node); 540 | 541 | 542 | BufferConfig m_bufferConfig; 543 | bool m_should_stop_thread{ false }; 544 | std::mutex m_mutex_cv; 545 | std::condition_variable m_cv; 546 | std::shared_ptr> m_tree; 547 | 548 | std::function m_nowFunction{DefaultClock}; 549 | std::function m_saveCallback{}; 550 | 551 | std::thread m_save_thread; 552 | matioCpp::CellArray m_description_cell_array; 553 | }; 554 | 555 | } // robometry 556 | 557 | #endif 558 | -------------------------------------------------------------------------------- /src/librobometry/include/robometry/Record.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #ifndef ROBOMETRY_RECORD_H 10 | #define ROBOMETRY_RECORD_H 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace robometry { 18 | 19 | /** 20 | * @brief A structure to represent a Record. 21 | * 22 | */ 23 | struct Record 24 | { 25 | double m_ts;/**< timestamp */ 26 | std::any m_datum;/**< the actual data of the record */ 27 | }; 28 | 29 | } // robometry 30 | 31 | #endif // ROBOMETRY_RECORD_H 32 | -------------------------------------------------------------------------------- /src/librobometry/include/robometry/TreeNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #ifndef ROBOMETRY_TREE_NODE_H 10 | #define ROBOMETRY_TREE_NODE_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace robometry { 23 | 24 | /** 25 | * @brief A class to represent the Node in Tree struct 26 | * 27 | */ 28 | template 29 | class TreeNode 30 | { 31 | public: 32 | /** 33 | * @brief Construct an empty Node of tree. 34 | */ 35 | TreeNode () = default; 36 | 37 | /** 38 | * @brief Construct an empty Node of tree containing a value. 39 | * 40 | * @param[in] _value An already initialized pointer containing the value stored in the nore 41 | */ 42 | TreeNode(std::shared_ptr _value) 43 | : m_value(_value) { 44 | assert(m_value); 45 | } 46 | 47 | /** 48 | * @brief Check if a child with a given name exist 49 | * 50 | * @param[in] name The name of the child 51 | * @return True if the child exist false otherwise. 52 | */ 53 | [[nodiscard]] 54 | bool childExists(const std::string& name) const { 55 | return m_children.find(name) != m_children.end(); 56 | } 57 | 58 | /** 59 | * @brief Add a new node in the tree. This node will be the child of the this node 60 | * 61 | * @param[in] name The name of the child 62 | * @param[in] node A pointer to the node 63 | * @return True if the child has been added, false otherwise. 64 | */ 65 | bool addChild(const std::string& name, std::shared_ptr> node) { 66 | 67 | if(this->childExists(name)) { 68 | std::cout << "The TreeNode named " << name << " already exist." << std::endl; 69 | return false; 70 | } 71 | 72 | if (node == nullptr) { 73 | std::cout << "The node should point to an already initialized memory." << std::endl; 74 | return false; 75 | } 76 | 77 | if (name.empty()) { 78 | std::cout << "The name of the node cannot be empty." << std::endl; 79 | return false; 80 | } 81 | 82 | // insert the new e element in the map 83 | const auto out = m_children.insert({name, node}); 84 | return out.second; 85 | } 86 | 87 | /** 88 | * @brief Get a child from the node. 89 | * 90 | * @param[in] name The name of the child. 91 | * @return A pointer to TreeNode, if the child is not found the weak pointer cannot be locked. 92 | */ 93 | std::weak_ptr> getChild(const std::string& name) const { 94 | if(this->childExists(name)) { 95 | return m_children.at(name); 96 | } 97 | 98 | return std::make_shared>(); 99 | } 100 | 101 | /** 102 | * @brief Get the value stored in the node. 103 | * 104 | * @return A pointer to the value stored in the node. 105 | */ 106 | std::shared_ptr getValue() { 107 | return m_value; 108 | } 109 | 110 | /** 111 | * @brief Get the map representing all the children associated to the node. 112 | * 113 | * @return The map of the children 114 | */ 115 | const std::unordered_map>& getChildren() const { 116 | return m_children; 117 | } 118 | 119 | 120 | /** 121 | * @brief Return a standard text representation of the content of the node. 122 | * 123 | * @return a string containing the standard text representation of the content of the object. 124 | */ 125 | [[nodiscard]] 126 | std::string toString(const std::string& name= ".", const unsigned int depth = 0) const { 127 | std::ostringstream oss; 128 | for (unsigned int i = 0 ; i < depth ; i++) { 129 | if (i != depth-1) { 130 | oss << " "; 131 | } else { 132 | oss << "|-- "; 133 | } 134 | } 135 | oss << name << std::endl; 136 | for (const auto & [key, child] : m_children) { 137 | oss << child->toString(key, depth + 1); 138 | } 139 | 140 | return oss.str(); 141 | } 142 | 143 | /** 144 | * @brief Check if the node is empty 145 | * 146 | * @return true if a node is empty 147 | */ 148 | [[nodiscard]] 149 | bool empty() const { 150 | return m_children.empty() && m_value == nullptr; 151 | } 152 | 153 | /** 154 | * Split a string using the separator 155 | * 156 | * @param[in] input the stribg that should be split 157 | * @return an std::vector containing the substrings 158 | */ 159 | static std::vector splitString(const std::string& input) { 160 | std::regex re(stringSeparator); 161 | std::sregex_token_iterator first{input.begin(), input.end(), re, -1}, last; 162 | return {first, last}; 163 | }; 164 | 165 | static std::string stringSeparator; /**< The string separator the default value is :: */ 166 | 167 | private: 168 | 169 | std::shared_ptr m_value; 170 | std::unordered_map> m_children; 171 | }; 172 | 173 | template 174 | std::string TreeNode::stringSeparator = "::"; 175 | 176 | 177 | /** 178 | * @brief Add a new leaf in the tree 179 | * 180 | * @param[in] nodes A span representing a list of nodes 181 | * @param[in] element The value stored by the leaf 182 | * @param[in] treeNode a pointer to a tree node 183 | * @return True in case of success false otherwise 184 | */ 185 | template 186 | bool addLeaf(matioCpp::Span nodes, 187 | std::shared_ptr element, 188 | std::shared_ptr> treeNode) { 189 | 190 | assert(nodes.begin() != nodes.end()); 191 | 192 | // get the first node in the in nodes 193 | const std::string node = *nodes.begin(); 194 | 195 | // if the size of the node is equal to 1 means that this is a leaf and so it will contain 196 | // the element 197 | if(nodes.size() == 1) { 198 | if(!treeNode->addChild(node, std::make_shared>(element))) { 199 | return false; 200 | } 201 | return true; 202 | } 203 | 204 | // create a new child 205 | if (!treeNode->childExists(node)) { 206 | if (!treeNode->addChild(node, std::make_shared>())) { 207 | return false; 208 | } 209 | } 210 | 211 | // this is always true since the child has been just added 212 | assert(treeNode->getChild(node).lock() != nullptr); 213 | 214 | // propagate the leaf creation to the child 215 | return addLeaf(nodes.subspan(1), element, treeNode->getChild(node).lock()); 216 | } 217 | 218 | /** 219 | * @brief Add a new leaf in the tree 220 | * 221 | * @param[in] name A string containing the address of the leaf. Use the TreeNode::stringSeparator 222 | * to define multiple nodes 223 | * @param[in] element The content of the leaf 224 | * @param[in] treeNode a pointer to a tree node 225 | * @return True in case of success false otherwise 226 | */ 227 | template 228 | bool addLeaf(const std::string& name, 229 | std::shared_ptr element, 230 | std::shared_ptr> treeNode) { 231 | // split the string using the separator 232 | const std::vector nodes = TreeNode::splitString(name); 233 | 234 | // build the leaf 235 | return addLeaf(nodes, element, treeNode); 236 | } 237 | 238 | /** 239 | * @brief Get the leaf from a node. 240 | * 241 | * @param[in] nodes A span representing a list of nodes 242 | * @param[in] treeNode a pointer to a tree node 243 | * @return A pointer to TreeNode, if the child is not found the weak pointer cannot be locked. 244 | */ 245 | template 246 | std::weak_ptr> getLeaf(matioCpp::Span nodes, 247 | std::shared_ptr> treeNode) { 248 | if (treeNode == nullptr) { 249 | return std::make_shared>(); 250 | } 251 | 252 | // check that the nodes is not an empty span 253 | assert(nodes.begin() != nodes.end()); 254 | 255 | // get the name of the node 256 | const std::string nodeName = *nodes.begin(); 257 | 258 | // Try to find the child named nodeName if not found a not lockable weak_ptr is returned 259 | auto ptr = treeNode->getChild(nodeName).lock(); 260 | if (ptr == nullptr) { 261 | return std::make_shared>(); 262 | } 263 | 264 | // if the size of the nodes is equal to one the child is actually the leaf 265 | if (nodes.size() == 1) { 266 | return ptr; 267 | } 268 | 269 | return getLeaf(nodes.subspan(1), ptr); 270 | } 271 | 272 | /** 273 | * @brief Get the leaf from a node. 274 | * 275 | * @param[in] name A string containing the address of the leaf. Use the TreeNode::stringSeparator 276 | * to define multiple nodes 277 | * @param[in] treeNode a pointer to a tree node 278 | * @return A pointer to TreeNode, if the child is not found the weak pointer cannot be locked. 279 | */ 280 | template 281 | std::weak_ptr> getLeaf(const std::string& name, 282 | std::shared_ptr> treeNode) { 283 | const std::vector nodes = TreeNode::splitString(name); 284 | return getLeaf(nodes, treeNode); 285 | } 286 | 287 | } // robometry 288 | 289 | #endif 290 | -------------------------------------------------------------------------------- /src/librobometry/src/Buffer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2022 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #include 10 | 11 | robometry::Buffer::Buffer(size_t num_elements): m_buffer_ptr(std::make_shared>(num_elements)) 12 | { 13 | 14 | } 15 | 16 | void robometry::Buffer::push_back(const Record &elem) 17 | { 18 | m_buffer_ptr->push_back(elem); 19 | } 20 | 21 | void robometry::Buffer::push_back(Record &&elem) 22 | { 23 | m_buffer_ptr->push_back(std::move(elem)); 24 | } 25 | 26 | size_t robometry::Buffer::getBufferFreeSpace() const { 27 | return m_buffer_ptr->capacity() - m_buffer_ptr->size(); 28 | } 29 | 30 | size_t robometry::Buffer::size() const { 31 | return m_buffer_ptr->size(); 32 | } 33 | 34 | size_t robometry::Buffer::capacity() const { 35 | return m_buffer_ptr->capacity(); 36 | 37 | } 38 | 39 | bool robometry::Buffer::empty() const { 40 | return m_buffer_ptr->empty(); 41 | } 42 | 43 | void robometry::Buffer::resize(size_t new_size) { 44 | return m_buffer_ptr->resize(new_size); 45 | } 46 | 47 | void robometry::Buffer::set_capacity(size_t new_size) { 48 | return m_buffer_ptr->set_capacity(new_size); 49 | } 50 | 51 | bool robometry::Buffer::full() const { 52 | return m_buffer_ptr->full(); 53 | } 54 | 55 | robometry::Buffer::const_iterator robometry::Buffer::begin() const noexcept { 56 | return m_buffer_ptr->begin(); 57 | } 58 | 59 | robometry::Buffer::iterator robometry::Buffer::end() noexcept { 60 | return m_buffer_ptr->end(); 61 | } 62 | 63 | robometry::Buffer::iterator robometry::Buffer::begin() noexcept { 64 | return m_buffer_ptr->begin(); 65 | } 66 | 67 | robometry::Buffer::const_iterator robometry::Buffer::end() const noexcept { 68 | return m_buffer_ptr->end(); 69 | } 70 | 71 | void robometry::Buffer::clear() noexcept { 72 | return m_buffer_ptr->clear(); 73 | 74 | } 75 | 76 | std::shared_ptr > robometry::Buffer::getBufferSharedPtr() const { 77 | return m_buffer_ptr; 78 | } 79 | -------------------------------------------------------------------------------- /src/librobometry/src/BufferConfig.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace matioCpp { 15 | NLOHMANN_JSON_SERIALIZE_ENUM( FileVersion, { 16 | {FileVersion::Undefined, "undefined"}, 17 | {FileVersion::MAT4, "v4"}, 18 | {FileVersion::MAT5, "v5"}, 19 | {FileVersion::MAT7_3, "v7_3"}, 20 | {FileVersion::Default, "default"}, 21 | }) 22 | } 23 | 24 | namespace robometry { 25 | 26 | ChannelInfo::ChannelInfo(const std::string& name, 27 | const dimensions_t& dimensions, 28 | const elements_names_t& elements_names, 29 | const units_of_measure_t& units_of_measure) : name(name), 30 | dimensions(dimensions), 31 | elements_names(elements_names), 32 | units_of_measure(units_of_measure) 33 | { 34 | const unsigned int elements = std::accumulate(dimensions.begin(), 35 | dimensions.end(), 36 | 1, 37 | std::multiplies<>()); 38 | if(elements_names.empty()){ 39 | for (unsigned int i = 0; i < elements; i++) { 40 | this->elements_names.push_back("element_" + std::to_string(i)); 41 | } 42 | } 43 | else { 44 | if(elements != elements_names.size()) { 45 | std::cout << "[ChannelInfo::ChannelInfo] The size of the vector elements_names is " 46 | << "different from the expected one. Expected: " << elements 47 | << " Passed: " << elements_names.size() << std::endl; 48 | } 49 | } 50 | 51 | if(units_of_measure.empty()) { 52 | this->units_of_measure.resize(elements, "n.d."); 53 | } 54 | else { 55 | // If just one string is specified, let assume that all the data has 56 | // the same unit of measure 57 | if (units_of_measure.size()==1) { 58 | this->units_of_measure.resize(elements, units_of_measure[0]); 59 | } 60 | else if (elements != units_of_measure.size()) { 61 | std::cout << "[ChannelInfo::ChannelInfo] The size of the vector units_of_measure is " 62 | << "different from the expected one. Expected: " << elements 63 | << " Passed: " << units_of_measure.size() << std::endl; 64 | } 65 | } 66 | 67 | } 68 | 69 | void to_json(nlohmann::json& j, const ChannelInfo& info) 70 | { 71 | j = nlohmann::json{{"name", info.name}, 72 | {"dimensions", info.dimensions}, 73 | {"elements_names", info.elements_names}, 74 | {"units_of_measure", info.units_of_measure}}; 75 | } 76 | 77 | void from_json(const nlohmann::json& j, ChannelInfo& info) 78 | { 79 | j.at("name").get_to(info.name); 80 | j.at("dimensions").get_to(info.dimensions); 81 | j.at("elements_names").get_to(info.elements_names); 82 | j.at("units_of_measure").get_to(info.units_of_measure); 83 | } 84 | 85 | // This expects that the name of the json keyword is the same of the relative variable 86 | NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(BufferConfig, yarp_robot_name, description_list, path, filename, n_samples, save_period, data_threshold, auto_save, save_periodically, channels, enable_compression, file_indexing, mat_file_version) 87 | } 88 | bool bufferConfigFromJson(robometry::BufferConfig& bufferConfig, const std::string& config_filename) { 89 | // read a JSON file 90 | std::ifstream input_stream(config_filename); 91 | if (!input_stream.is_open()) { 92 | std::cout << "Failed to open " << config_filename << std::endl; 93 | return false; 94 | } 95 | nlohmann::json jason_file; 96 | input_stream >> jason_file; 97 | bufferConfig = jason_file.get(); 98 | input_stream.close(); 99 | return true; 100 | } 101 | 102 | bool bufferConfigToJson(const robometry::BufferConfig& bufferConfig, const std::string& config_filename) { 103 | // write to a JSON file 104 | std::ofstream out_stream(config_filename); 105 | if (!out_stream.is_open()) { 106 | std::cout << "Failed to open " << config_filename << std::endl; 107 | return false; 108 | } 109 | nlohmann::json j = bufferConfig; 110 | out_stream << j; 111 | out_stream.close(); 112 | return true; 113 | } 114 | -------------------------------------------------------------------------------- /src/librobometry/src/BufferManager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2022 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #include 10 | 11 | robometry::BufferManager::BufferManager() { 12 | m_tree = std::make_shared>(); 13 | } 14 | 15 | robometry::BufferManager::BufferManager(const BufferConfig &_bufferConfig) { 16 | m_tree = std::make_shared>(); 17 | bool ok = configure(_bufferConfig); 18 | assert(ok); 19 | // For avoiding warnings in Release 20 | ROBOMETRY_UNUSED(ok) 21 | } 22 | 23 | robometry::BufferManager::~BufferManager() { 24 | if (m_save_thread.joinable()) { 25 | // This additional brackets are needed for make unique_lock out of scope before the join 26 | { 27 | std::unique_lock lk_cv(m_mutex_cv); 28 | m_should_stop_thread = true; 29 | m_cv.notify_one(); // Wake up the thread in order to close it 30 | } 31 | m_save_thread.join(); 32 | } 33 | if (m_bufferConfig.auto_save) { 34 | std::string fileName; 35 | saveToFile(fileName); 36 | if (m_saveCallback) 37 | { 38 | m_saveCallback(fileName, SaveCallbackSaveMethod::last_call); 39 | } 40 | } 41 | } 42 | 43 | bool robometry::BufferManager::enablePeriodicSave(double _save_period) { 44 | // If it is not joinable means it is not running 45 | if (!m_save_thread.joinable()) { 46 | m_bufferConfig.save_periodically = true; 47 | m_bufferConfig.save_period = _save_period; 48 | m_save_thread = std::thread(&BufferManager::periodicSave, this); 49 | return true; 50 | } 51 | return false; 52 | } 53 | 54 | bool robometry::BufferManager::configure(const BufferConfig &_bufferConfig) { 55 | bool ok{ true }; 56 | if (_bufferConfig.filename == "") 57 | { 58 | std::cout << "The filename cannot be empty." << std::endl; 59 | return false; 60 | } 61 | set_capacity(_bufferConfig.n_samples); 62 | m_bufferConfig = _bufferConfig; 63 | if (!_bufferConfig.channels.empty()) { 64 | ok = ok && addChannels(_bufferConfig.channels); 65 | } 66 | if (ok && _bufferConfig.save_periodically) { 67 | ok = ok && enablePeriodicSave(_bufferConfig.save_period); 68 | } 69 | populateDescriptionCellArray(); 70 | if (!m_bufferConfig.path.empty() && !robometry_fs::exists(m_bufferConfig.path)) { 71 | std::error_code ec; 72 | auto dir_created = robometry_fs::create_directory(m_bufferConfig.path, ec); 73 | if (!dir_created) { 74 | std::cout << m_bufferConfig.path << " does not exists, and it was not possible to create it." << std::endl; 75 | return false; 76 | } 77 | } 78 | // TODO ROLL BACK IN CASE OF FAILURE 79 | return ok; 80 | } 81 | 82 | robometry::BufferConfig robometry::BufferManager::getBufferConfig() const { 83 | return m_bufferConfig; 84 | } 85 | 86 | void robometry::BufferManager::setFileName(const std::string &filename) { 87 | m_bufferConfig.filename = filename; 88 | return; 89 | } 90 | 91 | void robometry::BufferManager::setDefaultPath(const std::string &path) { 92 | m_bufferConfig.path = path; 93 | return; 94 | } 95 | 96 | void robometry::BufferManager::enableCompression(bool enable_compression) { 97 | m_bufferConfig.enable_compression = enable_compression; 98 | return; 99 | } 100 | 101 | void robometry::BufferManager::setDescriptionList(const std::vector &description_list) { 102 | m_bufferConfig.description_list = description_list; 103 | populateDescriptionCellArray(); 104 | return; 105 | } 106 | 107 | void robometry::BufferManager::resize(size_t new_size) { 108 | this->resize(new_size, m_tree); 109 | m_bufferConfig.n_samples = new_size; 110 | return; 111 | } 112 | 113 | void robometry::BufferManager::set_capacity(size_t new_size) { 114 | this->set_capacity(new_size, m_tree); 115 | m_bufferConfig.n_samples = new_size; 116 | return; 117 | } 118 | 119 | bool robometry::BufferManager::addChannel(const ChannelInfo &channel) { 120 | auto buffInfo = std::make_shared(); 121 | buffInfo->m_buffer = Buffer(m_bufferConfig.n_samples); 122 | buffInfo->m_dimensions = channel.dimensions; 123 | 124 | buffInfo->m_dimensions_factorial = std::accumulate(channel.dimensions.begin(), 125 | channel.dimensions.end(), 126 | 1, 127 | std::multiplies<>()); 128 | 129 | buffInfo->m_elements_names = channel.elements_names; 130 | buffInfo->m_units_of_measure = channel.units_of_measure; 131 | 132 | const bool ok = addLeaf(channel.name, buffInfo, m_tree); 133 | if(ok) { 134 | m_bufferConfig.channels.push_back(channel); 135 | } 136 | else { 137 | std::cout << "Failed to add channel " << channel.name << std::endl; 138 | } 139 | return ok; 140 | } 141 | 142 | bool robometry::BufferManager::addChannels(const std::vector &channels) { 143 | if (channels.empty()) { 144 | return false; 145 | } 146 | bool ret{ true }; 147 | for (const auto& c : channels) { 148 | ret = ret && addChannel(c); 149 | } 150 | return ret; 151 | } 152 | 153 | bool robometry::BufferManager::saveToFile(bool flush_all) { 154 | std::string dummy_file_name; 155 | return saveToFile(dummy_file_name, flush_all); 156 | } 157 | 158 | bool robometry::BufferManager::saveToFile(std::string &file_name_path, bool flush_all) { 159 | 160 | // now we initialize the proto-timeseries structure 161 | std::vector signalsVect, descrListVect; 162 | // and the matioCpp struct for these signals 163 | // Add the description 164 | if (m_description_cell_array.isValid()) { 165 | signalsVect.emplace_back(m_description_cell_array); 166 | } 167 | 168 | signalsVect.emplace_back(matioCpp::String("yarp_robot_name", m_bufferConfig.yarp_robot_name)); 169 | 170 | // we have to force the flush. 171 | flush_all = flush_all || (m_bufferConfig.data_threshold > m_bufferConfig.n_samples); 172 | for (auto& [node_name, node] : m_tree->getChildren()) { 173 | 174 | // now we create the vector that stores different signals (in case we had more than one) 175 | signalsVect.emplace_back(this->createTreeStruct(node_name, node, flush_all)); 176 | } 177 | 178 | // This means that no variables are logged, we have only the description_list (if set) and the yarp_robot_name 179 | if (signalsVect.size() == static_cast(1 + m_description_cell_array.isValid())) { 180 | return false; 181 | } 182 | 183 | matioCpp::Struct timeSeries(m_bufferConfig.filename, signalsVect); 184 | // and finally we write the file 185 | // since we might save several files, we need to index them 186 | file_name_path = m_bufferConfig.filename + "_" + this->fileIndex(); 187 | if (!m_bufferConfig.path.empty()) { 188 | file_name_path = m_bufferConfig.path + file_name_path; 189 | } 190 | std::string new_file = file_name_path + ".mat"; 191 | assert(!matioCpp::File::Exists(new_file) && "A file with the same name already exists."); 192 | matioCpp::File file = matioCpp::File::Create(new_file, m_bufferConfig.mat_file_version); 193 | assert(file.isOpen() && "Failed to open the specified file."); 194 | bool ok = file.write(timeSeries, m_bufferConfig.enable_compression ? matioCpp::Compression::zlib : matioCpp::Compression::None); 195 | 196 | if (!ok) 197 | { 198 | std::cout << "An error occurred while saving the data to the file." << std::endl; 199 | } 200 | 201 | return ok; 202 | } 203 | 204 | bool robometry::BufferManager::setNowFunction(std::function now) 205 | { 206 | if (now == nullptr) { 207 | std::cout << "Not valid clock function." << std::endl; 208 | return false; 209 | } 210 | 211 | m_nowFunction = now; 212 | return true; 213 | } 214 | 215 | bool robometry::BufferManager::setSaveCallback(std::function saveCallback) 216 | { 217 | if (saveCallback == nullptr) { 218 | std::cout << "Not valid saveCallback function." << std::endl; 219 | return false; 220 | } 221 | 222 | m_saveCallback = saveCallback;; 223 | return true; 224 | } 225 | 226 | double robometry::BufferManager::DefaultClock() { 227 | return std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); 228 | } 229 | 230 | void robometry::BufferManager::periodicSave() 231 | { 232 | std::unique_lock lk_cv(m_mutex_cv); 233 | 234 | auto timeout = std::chrono::duration(m_bufferConfig.save_period); 235 | // For avoiding spurious wake up, the lambda check that the threads wake up only if we are trying to close 236 | // (additionally to the timeout expiration) 237 | while (!(m_cv.wait_for(lk_cv, timeout, [this](){return m_should_stop_thread;}))) 238 | { 239 | if (!m_tree->empty()) // if there are channels 240 | { 241 | std::string fileName; 242 | saveToFile(fileName, false); 243 | if (m_saveCallback) 244 | { 245 | m_saveCallback(fileName, SaveCallbackSaveMethod::periodic); 246 | } 247 | } 248 | } 249 | } 250 | 251 | matioCpp::Struct robometry::BufferManager::createTreeStruct(const std::string &node_name, std::shared_ptr > tree_node, bool flush_all) { 252 | const auto& children = tree_node->getChildren(); 253 | if (children.size() == 0) { 254 | return createElementStruct(node_name, tree_node->getValue(), flush_all); 255 | } 256 | 257 | matioCpp::Struct tmp(node_name); 258 | for (const auto& [child_name, child] : tree_node->getChildren()) { 259 | tmp.setField(this->createTreeStruct(child_name, child, flush_all)); 260 | } 261 | 262 | return tmp; 263 | } 264 | 265 | matioCpp::Struct robometry::BufferManager::createElementStruct(const std::string &var_name, std::shared_ptr buffInfo, bool flush_all) const { 266 | 267 | assert(buffInfo); 268 | 269 | std::scoped_lock lock{ buffInfo->m_buff_mutex }; 270 | if (buffInfo->m_buffer.empty()) { 271 | std::cout << var_name << " does not contain data, skipping" << std::endl; 272 | return matioCpp::Struct(var_name); 273 | } 274 | 275 | if (!flush_all && buffInfo->m_buffer.size() < m_bufferConfig.data_threshold) { 276 | std::cout << var_name << " does not contain enought data, skipping" << std::endl; 277 | return matioCpp::Struct(var_name); 278 | } 279 | 280 | // the number of timesteps is the size of our collection 281 | auto num_timesteps = buffInfo->m_buffer.size(); 282 | 283 | assert(buffInfo->m_convert_to_matioCpp); 284 | // We concatenate all the data of the buffer into a single variable 285 | matioCpp::Variable data = buffInfo->m_convert_to_matioCpp("data"); 286 | 287 | //We construct the timestamp vector 288 | matioCpp::Vector timestamps("timestamps", num_timesteps); 289 | size_t i = 0; 290 | for (auto& _cell : buffInfo->m_buffer) { 291 | timestamps[i] = _cell.m_ts; 292 | ++i; 293 | } 294 | assert(i == buffInfo->m_buffer.size()); 295 | 296 | //Clear the buffer, we don't need it anymore 297 | buffInfo->m_buffer.clear(); 298 | 299 | //Create the set of variables to be used in the output struct 300 | std::vector var_data; 301 | 302 | // now we create the vector for the dimensions 303 | dimensions_t fullDimensions = buffInfo->m_dimensions; 304 | fullDimensions.push_back(num_timesteps); 305 | 306 | var_data.emplace_back(data); // Data 307 | 308 | var_data.emplace_back(matioCpp::make_variable("dimensions", fullDimensions)); // dimensions vector 309 | var_data.emplace_back(matioCpp::make_variable("elements_names", buffInfo->m_elements_names)); // elements names 310 | var_data.emplace_back(matioCpp::make_variable("units_of_measure", buffInfo->m_units_of_measure)); // units_of_measure 311 | 312 | var_data.emplace_back(matioCpp::String("name", var_name)); // name of the signal 313 | var_data.emplace_back(timestamps); 314 | 315 | return matioCpp::Struct(var_name, var_data); 316 | } 317 | 318 | std::string robometry::BufferManager::fileIndex() const { 319 | if (m_bufferConfig.file_indexing == "time_since_epoch") { 320 | return std::to_string(m_nowFunction()); 321 | } 322 | std::time_t t = std::time(nullptr); 323 | std::tm tm = *std::localtime(&t); 324 | std::stringstream time; 325 | time << std::put_time(&tm, m_bufferConfig.file_indexing.c_str()); 326 | return time.str(); 327 | } 328 | 329 | void robometry::BufferManager::populateDescriptionCellArray() { 330 | if (m_bufferConfig.description_list.empty()) 331 | return; 332 | std::vector descrListVect; 333 | for (const auto& str : m_bufferConfig.description_list) { 334 | descrListVect.emplace_back(matioCpp::String("useless_name",str)); 335 | } 336 | matioCpp::CellArray description_list("description_list", { m_bufferConfig.description_list.size(), 1 }, descrListVect); 337 | m_description_cell_array = description_list; 338 | } 339 | 340 | void robometry::BufferManager::resize(size_t new_size, std::shared_ptr > node) { 341 | 342 | // resize the variable 343 | auto variable = node->getValue(); 344 | if (variable != nullptr) { 345 | variable->m_buffer.resize(new_size); 346 | } 347 | 348 | for (auto& [var_name, child] : node->getChildren()) { 349 | this->resize(new_size, child); 350 | } 351 | } 352 | 353 | void robometry::BufferManager::set_capacity(size_t new_size, std::shared_ptr > node) { 354 | 355 | // resize the variable 356 | auto variable = node->getValue(); 357 | if (variable != nullptr) { 358 | variable->m_buffer.set_capacity(new_size); 359 | } 360 | 361 | for (auto& [var_name, child] : node->getChildren()) { 362 | this->set_capacity(new_size, child); 363 | } 364 | } 365 | -------------------------------------------------------------------------------- /src/telemetryDeviceDumper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | yarp_configure_plugins_installation(robometry) 8 | 9 | yarp_prepare_plugin(telemetryDeviceDumper 10 | CATEGORY device 11 | TYPE robometry::TelemetryDeviceDumper 12 | INCLUDE TelemetryDeviceDumper.h 13 | DEFAULT ON) 14 | 15 | if(ENABLE_telemetryDeviceDumper) 16 | 17 | yarp_add_plugin(yarp_telemetryDeviceDumper) 18 | 19 | if(MSVC) 20 | add_definitions(-D_USE_MATH_DEFINES) 21 | endif() 22 | 23 | target_sources(yarp_telemetryDeviceDumper PRIVATE TelemetryDeviceDumper.cpp 24 | TelemetryDeviceDumper.h) 25 | 26 | target_link_libraries(yarp_telemetryDeviceDumper PRIVATE YARP::YARP_os 27 | YARP::YARP_dev 28 | robometry::robometry 29 | ICUB::iCubDev) 30 | list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os 31 | YARP_dev 32 | robometry) 33 | 34 | yarp_install(TARGETS yarp_telemetryDeviceDumper 35 | EXPORT YARP_${YARP_PLUGIN_MASTER} 36 | COMPONENT ${YARP_PLUGIN_MASTER} 37 | LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} 38 | ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} 39 | YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) 40 | 41 | set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) 42 | 43 | set_property(TARGET yarp_telemetryDeviceDumper PROPERTY FOLDER "Plugins/Device") 44 | endif() 45 | -------------------------------------------------------------------------------- /src/telemetryDeviceDumper/TelemetryDeviceDumper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT) 3 | * All rights reserved. 4 | * 5 | * This software may be modified and distributed under the terms of the 6 | * BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | */ 8 | 9 | #ifndef ROBOMETRY_TELEMETRYDEVICEDUMPER_H 10 | #define ROBOMETRY_TELEMETRYDEVICEDUMPER_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace robometry { 39 | 40 | 41 | struct TelemetryDeviceDumperSettings { 42 | 43 | bool logControlBoardQuantities{ false }; 44 | bool logIEncoders{ true }; 45 | bool logITorqueControl{ false }; 46 | bool logIMotorEncoders{ false }; 47 | bool logIControlMode{ false }; 48 | bool logIInteractionMode{ false }; 49 | bool logIPidControl{ false }; 50 | bool logIAmplifierControl{ false }; 51 | bool logIMotorTemperatures{ false }; 52 | bool logILocalization2D{ false }; 53 | bool logIRawValuesPublisher { false }; 54 | bool useRadians{ false }; 55 | bool saveBufferManagerConfiguration{ false }; 56 | std::string localizationRemoteName{ "" }; 57 | std::string rawValuesPublisherRemoteName { "" }; 58 | 59 | }; 60 | /** 61 | * @brief The `telemetryDeviceDumper` is a [yarp device](http://yarp.it/git-master/note_devices.html) 62 | * that has to be launched through the [`yarprobotinterface`](http://yarp.it/git-master/yarprobotinterface.html) 63 | * for dumping quantities from your robot(for example encoders, velocities etc) in base of what specified in the configuration. 64 | * 65 | * @section Params Parameters 66 | * | Parameter name | Type | Units | Default | Required | Description | 67 | * | -------- | -------- | -------- | -------- | -------- | -------- | 68 | * | `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. | 69 | * | `logIEncoders` | bool | - | true | No | Enable the log of `joints_state::positions`, `joints_state::velocities` and `joints_state::accelerations` (http://yarp.it/git-master/classyarp_1_1dev_1_1IEncoders.html) | 70 | * | `logITorqueControl` | bool | - | false | No | Enable the log of `joints_state::torques`(http://yarp.it/git-master/classyarp_1_1dev_1_1ITorqueControl.html). | 71 | * | `logIMotorEncoders` | bool | - | false | No | Enable the log of `motors_state::positions`, `motors_state::velocities` and `motors_state::accelerations` (http://yarp.it/git-master/classyarp_1_1dev_1_1IMotorEncoders.html). | 72 | * | `logIControlMode` | bool | - | false | No | Enable the log of `joints_state::control_mode` (http://yarp.it/git-master/classyarp_1_1dev_1_1IControlMode.html. | 73 | * | `logIInteractionMode` | bool | - | false | No | Enable the log of `joints_state::interaction_mode` (http://yarp.it/git-master/classyarp_1_1dev_1_1IInteractionMode.html. | 74 | * | `logIPidControl` | bool | - | false | No | Enable the log of `PIDs::position_error`, `PIDs::position_reference`, `PIDs::torque_error`, `PIDs::torque_reference`(http://yarp.it/git-master/classyarp_1_1dev_1_1IPidControl.html).| 75 | * | `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). | 76 | * | `logIMotorTemperatures` | bool | - | false | No | Enable the log of `motors_state::temperatures` available only with [yarp branch](https://github.com/ami-iit/yarp/tree/yarp-3.10.1-motor-temperature) (http://yarp.it/git-master/classyarp_1_1dev_1_1IMotor.html). | 77 | * | `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`). | 78 | * | `logILocalization2D` | bool | - | false | No | Enable the log of `odometry_data` (http://yarp.it/git-master/classyarp_1_1dev_1_1Nav2D_1_1ILocalization2D.html). | 79 | * | `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) | 80 | * | `saveBufferManagerConfiguration` | bool | - | false | No | Enable the save of the configuration of the BufferManager into `path`+ `"bufferConfig"` + `experimentName` + `".json"` | 81 | * | `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 | 82 | * | `experimentName` | string | - | - | Yes | Prefix of the files that will be saved. The files will be named: `experimentName`+`timestamp`+ `".mat"`. | 83 | * | `path` | string | - | - | No | Path of the folder where the data will be saved. | 84 | * | `n_samples` | size_t | - | - | Yes | The max number of samples contained in the circular buffer/s | 85 | * | `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. | 86 | * | `save_period` | double | seconds | - | Yes(if `save_periodically` is set to true) | The period in seconds of the save thread | 87 | * | `log_period` | double | seconds | 0.010 | No | The period in seconds of the logging thread. | 88 | * | `data_threshold` | size_t | - | 0 | No | The save thread saves to a file if there are at least `data_threshold` samples | 89 | * | `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 `robometry::BufferManager` | 90 | * | `yarp_robot_name` | string | - | "" | No | Name of the robot used during the experiment. | 91 | * 92 | * @section Mapping_mat_YARP Mapping .mat variables -> YARP interfaces 93 | * 94 | * | Variable name | YARP interface | 95 | * | -------------------- | -------------- | 96 | * | `joints_state::positions` | [`yarp::dev::IEncoders::getEncoders`](http://yarp.it/git-master/classyarp_1_1dev_1_1IEncoders.html#abcfe10041280b99c7c4384c4fd93a9dd) | 97 | * | `joints_state::velocities` | [`yarp::dev::IEncoders::getEncoderSpeeds`](http://yarp.it/git-master/classyarp_1_1dev_1_1IEncoders.html#ac84ae2f65f4a93b66827a3a424d1f743) | 98 | * | `joints_state::accelerations` | [`yarp::dev::IEncoders::getEncoderAccelerations`](http://yarp.it/git-master/classyarp_1_1dev_1_1IEncoders.html#a3bcb5fe5c6a5e15e57f4723bbe12c57a) | 99 | * | `joints_state::torques` | [`yarp::dev::ITorqueControl`](https://yarp.it//git-master/classyarp_1_1dev_1_1ITorqueControl.html#a45c1ad295b7fff91005a9f4564a35b2a)| 100 | * | `motors_state::positions` | [`yarp::dev::IMotorEncoders::getMotorEncoders`](http://yarp.it/git-master/classyarp_1_1dev_1_1IMotorEncoders.html#ac02fb05bb3e9ac9a381d41b59c38c412) | 101 | * | `motors_state::velocities` | [`yarp::dev::IMotorEncoders::getMotorEncoderSpeeds`](http://yarp.it/git-master/classyarp_1_1dev_1_1IMotorEncoders.html#a4624348cc129bfeb12ad0e6d2892b76c) | 102 | * | `motors_state::accelerations` | [`yarp::dev::IMotorEncoders::getMotorEncoderAccelerations`](http://yarp.it/git-master/classyarp_1_1dev_1_1IMotorEncoders.html#a9394d8b5cc4f3d58aeaa07c3fb9a6e6a) | 103 | * | `motors_state::pwm` | [`yarp::dev::IAmplifierControl::getPWM`](https://yarp.it//git-master/classyarp_1_1dev_1_1IAmplifierControl.html#a71ab30ccf182387bf6552d74f64ccfa5) | 104 | * | `motors_state::currents` | [`yarp::dev::IAmplifierControl::getCurrents`](https://yarp.it//git-master/classyarp_1_1dev_1_1IAmplifierControl.html#a60ab9c4fdc7f81bd136ad246a9dc57e8) | 105 | * | `motors_state::temperatures` | [`yarp::dev::IMotor::getTemperatures`](https://yarp.it//git-master/classyarp_1_1dev_1_1IMotor.html#a60ab9c4fdc7f81bd136ad246a9dc57e8) | 106 | * | `joints_state::control_mode` | [`yarp::dev::IControlMode::getControlModes`](http://yarp.it/git-master/classyarp_1_1dev_1_1IControlMode.html#a32f04715873a8099ec40671f65faff8d) | 107 | * | `joints_state::interaction_mode` | [`yarp::dev::IInteractionMode::getInteractionModes`](http://yarp.it/git-master/classyarp_1_1dev_1_1IInteractionMode.html#a6055ce20216f479da6c63807a4d11f54) | 108 | * | `PIDs::position_error` | [`yarp::dev::IPidControl::getPidErrors`](http://yarp.it/git-master/classyarp_1_1dev_1_1IPidControl.html#aea29e0fdf34f819ac69a3b940556ba28) | 109 | * | `PIDs::position_reference` | [`yarp::dev::IPidControl::getPidReferences`](http://yarp.it/git-master/classyarp_1_1dev_1_1IPidControl.html#a29e8f684a15d859229a9ae2902f886da) | 110 | * | `PIDs::torque_error` | [`yarp::dev::IPidControl::getPidErrors`](http://yarp.it/git-master/classyarp_1_1dev_1_1IPidControl.html#aea29e0fdf34f819ac69a3b940556ba28) | 111 | * | `PIDs::torque_reference` | [`yarp::dev::IPidControl::getPidReferences`](http://yarp.it/git-master/classyarp_1_1dev_1_1IPidControl.html#a29e8f684a15d859229a9ae2902f886da) | 112 | * | `PIDs::odometry_data ` | [`yarp::dev::Nav2D::ILocalization2D::getEstimatedOdometry`](http://yarp.it/git-master/classyarp_1_1dev_1_1Nav2D_1_1ILocalization2D.html#a02bff57282777ce7511b671abd4c95f0) | 113 | * | `raw_data_values` | [`iCub::debugLibrary::IRawValuesPublisher::getRawDataMap`] (https://github.com/robotology/icub-main/blob/devel/src/libraries/iCubDev/include/iCub/IRawValuesPublisher.h) 114 | * 115 | * @section Example_xml Example of xml 116 | * 117 | * Example of xml file for using it on the `iCub` robot: 118 | * 119 | * @code{.xml} 120 | * 121 | * 122 | * 123 | * 124 | * 125 | * (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) 126 | * true 127 | * true 128 | * true 129 | * true 130 | * true 131 | * false 132 | * true 133 | * false 134 | * false 135 | * true 136 | * test_telemetry 137 | * /home/icub/test_telemetry/ 138 | * 100000 139 | * true 140 | * 120.0 141 | * 0.010 142 | * 300 143 | * true 144 | * 145 | * 146 | * 147 | * 148 | * left_leg-eb7-j4_5-mc 149 | * right_leg-eb9-j4_5-mc 150 | * left_leg-eb6-j0_3-mc 151 | * right_leg-eb8-j0_3-mc 152 | * torso-eb5-j0_2-mc 153 | * right_arm-eb27-j4_7-mc 154 | * left_arm-eb24-j4_7-mc 155 | * right_arm-eb3-j0_3-mc 156 | * left_arm-eb1-j0_3-mc 157 | * head-eb20-j0_1-mc 158 | * head-eb21-j2_5-mc 159 | * 160 | * 161 | * 162 | * 163 | * 164 | * 165 | * 166 | * @endcode 167 | */ 168 | class TelemetryDeviceDumper : public yarp::dev::DeviceDriver, 169 | public yarp::dev::IMultipleWrapper, 170 | public yarp::os::PeriodicThread 171 | { 172 | public: 173 | TelemetryDeviceDumper(); 174 | ~TelemetryDeviceDumper() override; 175 | 176 | //DeviceDriver 177 | bool close() override; 178 | /** 179 | * Configure with a set of options. 180 | * @param config The options to use 181 | * @return true iff the object could be configured. 182 | */ 183 | bool open(yarp::os::Searchable& config) override; 184 | 185 | // IMultipleWrapper interface 186 | bool attachAll(const yarp::dev::PolyDriverList& device2attach) override; 187 | 188 | bool detachAll() override; 189 | 190 | void run() override; 191 | 192 | private: 193 | 194 | bool loadSettingsFromConfig(yarp::os::Searchable& config); 195 | bool attachAllControlBoards(const yarp::dev::PolyDriverList& p_list); 196 | bool openRemapperControlBoard(yarp::os::Searchable& config); 197 | void readSensors(); 198 | void readOdometryData(); 199 | void readRawValuesData(); 200 | void resizeBuffers(int size); 201 | bool configBufferManager(yarp::os::Searchable& config); 202 | /** Remapped controlboard containg the axes for which the joint torques are estimated */ 203 | yarp::dev::PolyDriver remappedControlBoard, localization2DClient, rawValuesPublisherClient; 204 | struct 205 | { 206 | yarp::dev::IEncoders* encs{nullptr}; 207 | yarp::dev::IMotorEncoders* imotenc{ nullptr }; 208 | yarp::dev::IPidControl* pid{ nullptr }; 209 | yarp::dev::IAmplifierControl* amp{nullptr}; 210 | yarp::dev::IControlMode* cmod{ nullptr }; 211 | yarp::dev::IInteractionMode* imod{ nullptr }; 212 | yarp::dev::ITorqueControl* itrq{ nullptr }; 213 | yarp::dev::IMultipleWrapper* multwrap{ nullptr }; 214 | yarp::dev::IMotor* imot{ nullptr }; 215 | } remappedControlBoardInterfaces; 216 | 217 | yarp::dev::Nav2D::ILocalization2D* iloc{nullptr}; 218 | 219 | iCub::debugLibrary::IRawValuesPublisher* iravap{ nullptr }; 220 | 221 | std::mutex deviceMutex; 222 | std::atomic correctlyConfigured{ false }, sensorsReadCorrectly{false}; 223 | std::vector jointPos, jointVel, jointAcc, jointPosErr, jointPosRef, 224 | jointTrqErr, jointTrqRef, jointPWM, jointCurr, jointTrq, 225 | motorEnc, motorVel, motorAcc, motorTemp, controlModes, interactionModes, 226 | odometryData; 227 | 228 | std::map> rawDataValuesMap; 229 | 230 | std::vector jointNames; 231 | TelemetryDeviceDumperSettings settings; 232 | robometry::BufferConfig m_bufferConfig; 233 | robometry::BufferManager bufferManager; 234 | 235 | 236 | }; 237 | } 238 | 239 | #endif // ROBOMETRY_TELEMETRYDEVICEDUMPER_H 240 | -------------------------------------------------------------------------------- /src/telemetryDeviceDumper/app/launch-telemetryDeviceDumper-icub_sim.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | /icubSim/torso 8 | /telemetryDeviceDumper/torso 9 | 10 | 11 | 12 | /icubSim/left_arm 13 | /telemetryDeviceDumper/left_arm 14 | 15 | 16 | 17 | /icubSim/right_arm 18 | /telemetryDeviceDumper/right_arm 19 | 20 | 21 | 22 | /icubSim/left_leg 23 | /telemetryDeviceDumper/left_leg 24 | 25 | 26 | 27 | /icubSim/right_leg 28 | /telemetryDeviceDumper/right_leg 29 | 30 | 31 | 32 | /icubSim/head 33 | /telemetryDeviceDumper/head 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/telemetryDeviceDumper/app/telemetryDeviceDumper.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | (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) 7 | true 8 | true 9 | true 10 | true 11 | true 12 | false 13 | true 14 | true 15 | test_telemetry 16 | /home/icub/test_telemetry/ 17 | 100000 18 | true 19 | 120.0 20 | 300 21 | true 22 | 23 | 24 | 25 | 26 | left_leg-eb7-j4_5-mc 27 | right_leg-eb9-j4_5-mc 28 | left_leg-eb6-j0_3-mc 29 | right_leg-eb8-j0_3-mc 30 | torso-eb5-j0_2-mc 31 | right_arm-eb27-j4_7-mc 32 | left_arm-eb24-j4_7-mc 33 | right_arm-eb3-j0_3-mc 34 | left_arm-eb1-j0_3-mc 35 | head-eb20-j0_1-mc 36 | head-eb21-j2_5-mc 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /src/telemetryDeviceDumper/app/telemetryDeviceDumper_sim.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | (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) 7 | true 8 | true 9 | /localizationServer 10 | true 11 | test_telemetry 12 | /home/icub/test_telemetry/ 13 | 100000 14 | true 15 | 120.0 16 | 300 17 | true 18 | 19 | 20 | 21 | 22 | left_leg_mc 23 | right_leg_mc 24 | torso_mc 25 | right_arm_mc 26 | left_arm_mc 27 | head_mc 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /src/telemetryDeviceDumper/app/yarprobotinterface.ini: -------------------------------------------------------------------------------- 1 | config ./launch-telemetryDeviceDumper-icub_sim.xml -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | if(ROBOMETRY_VALGRIND_TESTS) 8 | add_definitions(-DWITH_VALGRIND) 9 | endif() 10 | 11 | add_executable(bufferManager_test BufferManagerTest.cpp) 12 | if(ROBOMETRY_BENCHMARKING) 13 | target_compile_definitions(bufferManager_test PRIVATE -DCATCH_CONFIG_ENABLE_BENCHMARKING) 14 | endif() 15 | target_link_libraries(bufferManager_test PRIVATE Catch2::Catch2WithMain 16 | matioCpp::matioCpp 17 | Boost::boost 18 | robometry::robometry) 19 | 20 | include(CTest) 21 | include(Catch) 22 | function(robot_telemetry_catch_discover_tests _target) 23 | # Workaround to force catch_discover_tests to run tests under valgrind 24 | set_property(TARGET ${_target} PROPERTY CROSSCOMPILING_EMULATOR "${ROBOMETRY_TEST_LAUNCHER}") 25 | catch_discover_tests( 26 | ${_target} 27 | EXTRA_ARGS "-s" 28 | PROPERTIES 29 | TIMEOUT ${ROBOMETRY_TEST_TIMEOUT} 30 | SKIP_RETURN_CODE 254 31 | ) 32 | endfunction() 33 | 34 | 35 | robot_telemetry_catch_discover_tests(bufferManager_test) 36 | --------------------------------------------------------------------------------