├── .clang-format ├── .github └── workflows │ ├── cmake_ubuntu.yml │ └── ros2.yml ├── .gitignore ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── concepts.png ├── data_tamer_cpp ├── 3rdparty │ ├── ConcurrentQueue │ │ ├── blockingconcurrentqueue.h │ │ ├── concurrentqueue.h │ │ └── lightweightsemaphore.h │ └── mcap │ │ ├── CMakeLists.txt │ │ ├── cmake │ │ ├── FindLZ4.cmake │ │ └── FindZSTD.cmake │ │ ├── include │ │ └── mcap │ │ │ ├── crc32.hpp │ │ │ ├── errors.hpp │ │ │ ├── internal.hpp │ │ │ ├── intervaltree.hpp │ │ │ ├── mcap.hpp │ │ │ ├── read_job_queue.hpp │ │ │ ├── reader.hpp │ │ │ ├── reader.inl │ │ │ ├── types.hpp │ │ │ ├── types.inl │ │ │ ├── visibility.hpp │ │ │ ├── writer.hpp │ │ │ └── writer.inl │ │ └── mcap.cpp ├── CHANGELOG.rst ├── CMakeLists.txt ├── benchmarks │ ├── CMakeLists.txt │ └── data_tamer_benchmark.cpp ├── conanfile.py ├── data_tamerConfig.cmake.in ├── examples │ ├── CMakeLists.txt │ ├── T01_basic_example.cpp │ ├── T02_custom_types.cpp │ ├── T03_mcap_writer.cpp │ ├── geometry_types.hpp │ ├── mcap_1m_per_sec.cpp │ ├── mcap_reader.cpp │ └── ros2_publisher.cpp ├── include │ ├── data_tamer │ │ ├── channel.hpp │ │ ├── contrib │ │ │ └── SerializeMe.hpp │ │ ├── custom_types.hpp │ │ ├── data_sink.hpp │ │ ├── data_tamer.hpp │ │ ├── details │ │ │ └── locked_reference.hpp │ │ ├── logged_value.hpp │ │ ├── sinks │ │ │ ├── dummy_sink.hpp │ │ │ ├── mcap_sink.hpp │ │ │ └── ros2_publisher_sink.hpp │ │ ├── types.hpp │ │ └── values.hpp │ └── data_tamer_parser │ │ └── data_tamer_parser.hpp ├── package.xml ├── src │ ├── channel.cpp │ ├── data_sink.cpp │ ├── data_tamer.cpp │ ├── sinks │ │ ├── mcap_sink.cpp │ │ └── ros2_publisher_sink.cpp │ └── types.cpp └── tests │ ├── CMakeLists.txt │ ├── custom_types_tests.cpp │ ├── dt_tests.cpp │ └── parser_tests.cpp ├── data_tamer_logo.png ├── data_tamer_logo.xcf └── data_tamer_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg ├── Schema.msg ├── Schemas.msg └── Snapshot.msg └── package.xml /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: Inline 11 | AlwaysBreakTemplateDeclarations: true 12 | AlwaysBreakBeforeMultilineStrings: false 13 | BreakBeforeBinaryOperators: false 14 | BreakBeforeTernaryOperators: false 15 | BreakConstructorInitializers: BeforeComma 16 | BinPackParameters: true 17 | ColumnLimit: 90 18 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 19 | DerivePointerBinding: false 20 | PointerBindsToType: true 21 | ExperimentalAutoDetectBinPacking: false 22 | IndentCaseLabels: true 23 | MaxEmptyLinesToKeep: 1 24 | NamespaceIndentation: None 25 | ObjCSpaceBeforeProtocolList: true 26 | PenaltyBreakBeforeFirstCallParameter: 19 27 | PenaltyBreakComment: 60 28 | PenaltyBreakString: 1 29 | PenaltyBreakFirstLessLess: 1000 30 | PenaltyExcessCharacter: 1000 31 | PenaltyReturnTypeOnItsOwnLine: 90 32 | SpacesBeforeTrailingComments: 2 33 | Cpp11BracedListStyle: false 34 | Standard: Auto 35 | IndentWidth: 2 36 | TabWidth: 2 37 | UseTab: Never 38 | IndentFunctionDeclarationAfterType: false 39 | SpacesInParentheses: false 40 | SpacesInAngles: false 41 | SpaceInEmptyParentheses: false 42 | SpacesInCStyleCastParentheses: false 43 | SpaceAfterControlStatementKeyword: true 44 | SpaceBeforeAssignmentOperators: true 45 | SpaceBeforeParens: Never 46 | ContinuationIndentWidth: 4 47 | SortIncludes: false 48 | SpaceAfterCStyleCast: false 49 | ReflowComments: false 50 | 51 | # Configure each individual brace in BraceWrapping 52 | BreakBeforeBraces: Custom 53 | 54 | # Control of individual brace wrapping cases 55 | BraceWrapping: { 56 | AfterClass: 'true', 57 | AfterControlStatement: 'true', 58 | AfterEnum : 'true', 59 | AfterFunction : 'true', 60 | AfterNamespace : 'true', 61 | AfterStruct : 'true', 62 | AfterUnion : 'true', 63 | BeforeCatch : 'true', 64 | BeforeElse : 'true', 65 | IndentBraces : 'false', 66 | SplitEmptyFunction: 'false' 67 | } 68 | ... 69 | -------------------------------------------------------------------------------- /.github/workflows/cmake_ubuntu.yml: -------------------------------------------------------------------------------- 1 | 2 | name: cmake Ubuntu 3 | 4 | on: [push, pull_request] 5 | 6 | env: 7 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 8 | BUILD_TYPE: Debug 9 | 10 | jobs: 11 | build: 12 | # The CMake configure and build commands are platform agnostic and should work equally 13 | # well on Windows or Mac. You can convert this to a matrix build if you need 14 | # cross-platform coverage. 15 | # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix 16 | runs-on: ${{ matrix.os }} 17 | strategy: 18 | matrix: 19 | os: [ubuntu-22.04, ubuntu-24.04] 20 | 21 | steps: 22 | - uses: actions/checkout@v2 23 | 24 | # ubuntu 24.04 doesn't allow non-user pip install by default anymore 25 | # don't do this on your host computer, but this is fine in CI 26 | - name: pip break system packages 27 | if: matrix.os == 'ubuntu-24.04' 28 | run: python3 -m pip config set global.break-system-packages true 29 | 30 | - name: Install Conan 31 | id: conan 32 | uses: turtlebrowser/get-conan@main 33 | with: 34 | version: 2.0.13 35 | 36 | - name: Create default profile 37 | run: conan profile detect 38 | 39 | - name: Create Build Environment 40 | # Some projects don't allow in-source building, so create a separate build directory 41 | # We'll use this as our working directory for all subsequent commands 42 | run: cmake -E make_directory ${{github.workspace}}/build 43 | 44 | - name: Install conan dependencies 45 | working-directory: ${{github.workspace}}/build 46 | run: conan install ${{github.workspace}}/data_tamer_cpp/conanfile.py -s build_type=Debug --build=missing -s compiler.cppstd=gnu17 -of . 47 | 48 | - name: Configure CMake 49 | shell: bash 50 | working-directory: ${{github.workspace}}/build 51 | run: cmake ${{github.workspace}}/data_tamer_cpp -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE="./build/Debug/generators/conan_toolchain.cmake" -DCMAKE_CXX_FLAGS_DEBUG="-g -fprofile-arcs -ftest-coverage" 52 | 53 | - name: Build 54 | shell: bash 55 | working-directory: ${{github.workspace}}/build 56 | run: cmake --build . --config Debug 57 | 58 | - name: run test (Linux) 59 | working-directory: ${{github.workspace}}/build 60 | run: ctest -T test -T Coverage 61 | 62 | - name: Upload coverage reports to Codecov 63 | uses: codecov/codecov-action@v3 64 | with: 65 | directory: ${{github.workspace}}/build 66 | gcov_ignore: ${{github.workspace}}/3rdparty 67 | verbose: true 68 | env: 69 | CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} 70 | -------------------------------------------------------------------------------- /.github/workflows/ros2.yml: -------------------------------------------------------------------------------- 1 | name: ros2 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | strategy: 8 | matrix: 9 | env: 10 | - {ROS_DISTRO: humble, ROS_REPO: main} 11 | - {ROS_DISTRO: iron, ROS_REPO: main} 12 | - {ROS_DISTRO: jazzy, ROS_REPO: main} 13 | - {ROS_DISTRO: rolling, ROS_REPO: main} 14 | runs-on: ubuntu-latest 15 | steps: 16 | - uses: actions/checkout@v1 17 | - uses: 'ros-industrial/industrial_ci@master' 18 | env: ${{matrix.env}} 19 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # This file is used to ignore files which are generated 2 | # ---------------------------------------------------------------------------- 3 | 4 | *~ 5 | *.autosave 6 | *.a 7 | *.core 8 | *.moc 9 | *.o 10 | *.obj 11 | *.orig 12 | *.rej 13 | *.so 14 | *.so.* 15 | *_pch.h.cpp 16 | *_resource.rc 17 | *.qm 18 | .#* 19 | *.*# 20 | core 21 | !core/ 22 | tags 23 | .DS_Store 24 | .directory 25 | *.debug 26 | Makefile* 27 | *.prl 28 | *.app 29 | moc_*.cpp 30 | ui_*.h 31 | qrc_*.cpp 32 | Thumbs.db 33 | *.res 34 | *.rc 35 | /.qmake.cache 36 | /.qmake.stash 37 | 38 | # qtcreator generated files 39 | *.pro.user* 40 | CMakeLists.txt.user* 41 | 42 | # xemacs temporary files 43 | *.flc 44 | 45 | # Vim temporary files 46 | .*.swp 47 | 48 | # Visual Studio generated files 49 | *.ib_pdb_index 50 | *.idb 51 | *.ilk 52 | *.pdb 53 | *.sln 54 | *.suo 55 | *.vcproj 56 | *vcproj.*.*.user 57 | *.ncb 58 | *.sdf 59 | *.opensdf 60 | *.vcxproj 61 | *vcxproj.* 62 | 63 | # MinGW generated files 64 | *.Debug 65 | *.Release 66 | 67 | # Python byte code 68 | *.pyc 69 | 70 | # Binaries 71 | # -------- 72 | *.dll 73 | *.exe 74 | 75 | # Custom 76 | build* 77 | */CMakeUserPresets.json 78 | /perf.* 79 | /data_tamer/CMakeUserPresets.json 80 | .cache/* 81 | */.cache/* 82 | *.mcap 83 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | 2 | # To use: 3 | # 4 | # pre-commit run -a 5 | # 6 | # Or: 7 | # 8 | # pre-commit install # (runs every time you commit in git) 9 | # 10 | # To update this file: 11 | # 12 | # pre-commit autoupdate 13 | # 14 | # See https://github.com/pre-commit/pre-commit 15 | 16 | exclude: ^data_tamer_cpp/3rdparty/ 17 | repos: 18 | 19 | # Standard hooks 20 | - repo: https://github.com/pre-commit/pre-commit-hooks 21 | rev: v5.0.0 22 | hooks: 23 | - id: check-added-large-files 24 | - id: check-ast 25 | - id: check-case-conflict 26 | - id: check-docstring-first 27 | - id: check-merge-conflict 28 | - id: check-symlinks 29 | - id: check-xml 30 | - id: check-yaml 31 | - id: debug-statements 32 | - id: end-of-file-fixer 33 | exclude_types: [svg] 34 | - id: mixed-line-ending 35 | - id: trailing-whitespace 36 | exclude_types: [svg] 37 | - id: fix-byte-order-marker 38 | 39 | # CPP hooks 40 | - repo: https://github.com/pre-commit/mirrors-clang-format 41 | rev: v18.1.2 42 | hooks: 43 | - id: clang-format 44 | args: ['-fallback-style=none', '-i'] 45 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2023 Asunsus Surgical 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 7 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![Data Tamer](data_tamer_logo.png) 2 | 3 | [![cmake Ubuntu](https://github.com/facontidavide/data_tamer/actions/workflows/cmake_ubuntu.yml/badge.svg)](https://github.com/facontidavide/data_tamer/actions/workflows/cmake_ubuntu.yml) 4 | [![ros2](https://github.com/PickNikRobotics/data_tamer/actions/workflows/ros2.yml/badge.svg)](https://github.com/PickNikRobotics/data_tamer/actions/workflows/ros2.yml) 5 | [![codecov](https://codecov.io/gh/facontidavide/data_tamer/graph/badge.svg?token=D0wtsntWds)](https://codecov.io/gh/facontidavide/data_tamer) 6 | 7 | **DataTamer** is a library to log/trace numerical variables over time and 8 | takes periodic "snapshots" of their values, to later visualize them as **timeseries**. 9 | 10 | It works great with [PlotJuggler](https://github.com/facontidavide/PlotJuggler), 11 | the timeseries visualization tool (note: you will need PlotJuggler **3.8.2** or later). 12 | 13 | **DataTamer** is "fearless data logger" because you can record hundreds or **thousands of variables**: 14 | even 1 million points per second should have a fairly small CPU overhead. 15 | 16 | Since all the values are aggregated in a single "snapshot", it is usually meant to 17 | record data in a periodic loop (a very frequent use case, in robotics applications). 18 | 19 | Kudos to [pal_statistics](https://github.com/pal-robotics/pal_statistics), for inspiring this project. 20 | 21 | ## How it works 22 | 23 | ![architecture](concepts.png) 24 | 25 | DataTamer can be used to monitor multiple variables in your applications. 26 | 27 | **Channels** are used to take "snapshots" of a subset of variables at a given time. 28 | If you want to record at different frequencies, you can use different channels. 29 | 30 | DataTamer will forward the collected data to 1 or multiple **sinks**; 31 | a sink may save the information immediately in a file (currently, we support [MCAP](https://mcap.dev/)) 32 | or publish it using an inter-process communication, for instance, a ROS2 publisher. 33 | 34 | You can easily create your own, specialized sinks. 35 | 36 | Use [PlotJuggler](https://github.com/facontidavide/PlotJuggler) to 37 | visualize your logs offline or in real-time. 38 | 39 | ## Features 40 | 41 | - **Serialization schema is created at run-time**: no need to do code generation. 42 | - **Suitable for real-time applications**: very low latency (on the side of the callee). 43 | - **Multi-sink architecture**: recorded data can be forwarded to multiple "backends". 44 | - **Very low serialization overhead**, in the order of 1 bit per traced value. 45 | - The user can enable/disable traced variables at run-time. 46 | 47 | ## Limitations 48 | 49 | - Traced variables can not be added (registered) once the recording starts (first `takeSnapshot`). 50 | - Focused on periodic recording. Not the best option for sporadic, asynchronous events. 51 | - If you use `DataTamer::registerValue` you must be careful about the lifetime of the 52 | object. If you prefer a safer RAII interface, use `DataTamer::createLoggedValue` instead. 53 | 54 | # Examples 55 | 56 | ## Basic example 57 | 58 | ```cpp 59 | #include "data_tamer/data_tamer.hpp" 60 | #include "data_tamer/sinks/mcap_sink.hpp" 61 | 62 | int main() 63 | { 64 | // Multiple channels can use this sink. Data will be saved in mylog.mcap 65 | auto mcap_sink = std::make_shared("mylog.mcap"); 66 | 67 | // Create a channel and attach a sink. A channel can have multiple sinks 68 | auto channel = DataTamer::LogChannel::create("my_channel"); 69 | channel->addDataSink(mcap_sink); 70 | 71 | // You can register any arithmetic value. You are responsible for their lifetime! 72 | double value_real = 3.14; 73 | int value_int = 42; 74 | auto id1 = channel->registerValue("value_real", &value_real); 75 | auto id2 = channel->registerValue("value_int", &value_int); 76 | 77 | // If you prefer to use RAII, use this method instead 78 | // logged_real will unregister itself when it goes out of scope. 79 | auto logged_real = channel->createLoggedValue("my_real"); 80 | 81 | // Store the current value of all the registered values 82 | channel->takeSnapshot(); 83 | 84 | // You can disable (i.e., stop recording) a value like this 85 | channel->setEnabled(id1, false); 86 | // or, in the case of a LoggedValue 87 | logged_real->setEnabled(false); 88 | 89 | // The next snapshot will contain only [value_int], i.e. [id2], 90 | // since the other two were disabled 91 | channel->takeSnapshot(); 92 | } 93 | ``` 94 | ## How to register custom types 95 | 96 | Containers such as `std::vector` and `std::array` are supported out of the box. 97 | You can also register a custom type, as shown in the example below. 98 | 99 | ```cpp 100 | #include "data_tamer/data_tamer.hpp" 101 | #include "data_tamer/sinks/mcap_sink.hpp" 102 | #include "data_tamer/custom_types.hpp" 103 | 104 | // a custom type 105 | struct Point3D 106 | { 107 | double x; 108 | double y; 109 | double z; 110 | }; 111 | 112 | namespace DataTamer 113 | { 114 | template <> struct TypeDefinition 115 | { 116 | // Provide the name of the type 117 | std::string typeName() const { return "Point3D"; } 118 | // List all the member variables that you want to be saved (including their name) 119 | template void typeDef(Function& addField) 120 | { 121 | addField("x", &Point3D::x); 122 | addField("y", &Point3D::y); 123 | addField("z", &Point3D::z); 124 | } 125 | } 126 | } // end namespace DataTamer 127 | 128 | int main() 129 | { 130 | auto channel = DataTamer::LogChannel::create("my_channel"); 131 | channel->addDataSink(std::make_shared("mylog.mcap")); 132 | 133 | // Array/vectors are supported natively 134 | std::vector values = {1, 2, 3, 4}; 135 | channel->registerValue("values", &values); 136 | 137 | // Requires the implementation of DataTamer::TypeDefinition 138 | Point3D position = {0.1, -0.2, 0.3}; 139 | channel->registerValue("position", &position); 140 | 141 | // save the data as usual ... 142 | channel->takeSnapshot(); 143 | } 144 | ``` 145 | 146 | # Compilation 147 | 148 | ## Compiling with ROS2 149 | 150 | Just use colcon :) 151 | 152 | ## Compiling with Conan (not ROS2 support) 153 | 154 | Note that the ROS2 publisher will **NOT** be built when using this method. 155 | 156 | Assuming conan 2.x installed. From the source directory. 157 | 158 | **Release**: 159 | 160 | ``` 161 | conan install . -s compiler.cppstd=gnu17 --build=missing -s build_type=Release 162 | cmake -S . -B build/Release -DCMAKE_BUILD_TYPE=Release \ 163 | -DCMAKE_TOOLCHAIN_FILE="build/Release/generators/conan_toolchain.cmake" 164 | cmake --build build/Release --parallel 165 | ``` 166 | 167 | **Debug**: 168 | 169 | ``` 170 | conan install . -s compiler.cppstd=gnu17 --build=missing -s build_type=Debug 171 | cmake -S . -B build/Debug -DCMAKE_BUILD_TYPE=Debug \ 172 | -DCMAKE_TOOLCHAIN_FILE="build/Debug/generators/conan_toolchain.cmake" 173 | cmake --build build/Debug --parallel 174 | ``` 175 | 176 | # How to deserialize data recorded with DataTamer 177 | 178 | I will write more extensively about the serialization format used by DataTamer, but for the time being I 179 | created a single header file without external dependencies that you can just copy into your project: 180 | [data_tamer_parser.hpp](data_tamer_cpp/include/data_tamer_parser/data_tamer_parser.hpp) 181 | 182 | You can see how it is used in this example: [mcap_reader](data_tamer_cpp/examples/mcap_reader.cpp) 183 | -------------------------------------------------------------------------------- /concepts.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/data_tamer/ae41fff484dcda1ad96527c003ecef3f787067e6/concepts.png -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 3 | 4 | find_package(ZSTD REQUIRED) 5 | find_package(LZ4 REQUIRED) 6 | 7 | add_library(mcap_lib STATIC mcap.cpp) 8 | target_link_libraries(mcap_lib PUBLIC ${LZ4_LIBRARY} ${ZSTD_LIBRARY}) 9 | target_include_directories(mcap_lib 10 | PUBLIC 11 | $ 12 | ) 13 | 14 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/cmake/FindLZ4.cmake: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | # Finds liblz4. 16 | # 17 | # This module defines: 18 | # LZ4_FOUND 19 | # LZ4_INCLUDE_DIR 20 | # LZ4_LIBRARY 21 | # 22 | 23 | find_path(LZ4_INCLUDE_DIR NAMES lz4.h) 24 | 25 | find_library(LZ4_LIBRARY_DEBUG NAMES lz4d) 26 | find_library(LZ4_LIBRARY_RELEASE NAMES lz4) 27 | 28 | include(SelectLibraryConfigurations) 29 | SELECT_LIBRARY_CONFIGURATIONS(LZ4) 30 | 31 | include(FindPackageHandleStandardArgs) 32 | FIND_PACKAGE_HANDLE_STANDARD_ARGS( 33 | LZ4 DEFAULT_MSG 34 | LZ4_LIBRARY LZ4_INCLUDE_DIR 35 | ) 36 | 37 | if (LZ4_FOUND) 38 | message(STATUS "Found LZ4: ${LZ4_LIBRARY}") 39 | endif() 40 | 41 | mark_as_advanced(LZ4_INCLUDE_DIR LZ4_LIBRARY) 42 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/cmake/FindZSTD.cmake: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | # 16 | # - Try to find Facebook zstd library 17 | # This will define 18 | # ZSTD_FOUND 19 | # ZSTD_INCLUDE_DIR 20 | # ZSTD_LIBRARY 21 | # 22 | 23 | find_path(ZSTD_INCLUDE_DIR NAMES zstd.h) 24 | 25 | find_library(ZSTD_LIBRARY_DEBUG NAMES zstdd zstd_staticd) 26 | find_library(ZSTD_LIBRARY_RELEASE NAMES zstd zstd_static) 27 | 28 | include(SelectLibraryConfigurations) 29 | SELECT_LIBRARY_CONFIGURATIONS(ZSTD) 30 | 31 | include(FindPackageHandleStandardArgs) 32 | FIND_PACKAGE_HANDLE_STANDARD_ARGS( 33 | ZSTD DEFAULT_MSG 34 | ZSTD_LIBRARY ZSTD_INCLUDE_DIR 35 | ) 36 | 37 | if (ZSTD_FOUND) 38 | message(STATUS "Found Zstd: ${ZSTD_LIBRARY}") 39 | endif() 40 | 41 | mark_as_advanced(ZSTD_INCLUDE_DIR ZSTD_LIBRARY) 42 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/include/mcap/crc32.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace mcap::internal { 6 | 7 | /** 8 | * Compute CRC32 lookup tables as described at: 9 | * https://github.com/komrad36/CRC#option-6-1-byte-tabular 10 | * 11 | * An iteration of CRC computation can be performed on 8 bits of input at once. By pre-computing a 12 | * table of the values of CRC(?) for all 2^8 = 256 possible byte values, during the final 13 | * computation we can replace a loop over 8 bits with a single lookup in the table. 14 | * 15 | * For further speedup, we can also pre-compute the values of CRC(?0) for all possible bytes when a 16 | * zero byte is appended. Then we can process two bytes of input at once by computing CRC(AB) = 17 | * CRC(A0) ^ CRC(B), using one lookup in the CRC(?0) table and one lookup in the CRC(?) table. 18 | * 19 | * The same technique applies for any number of bytes to be processed at once, although the speed 20 | * improvements diminish. 21 | * 22 | * @param Polynomial The binary representation of the polynomial to use (reversed, i.e. most 23 | * significant bit represents x^0). 24 | * @param NumTables The number of bytes of input that will be processed at once. 25 | */ 26 | template 27 | struct CRC32Table { 28 | private: 29 | std::array table = {}; 30 | 31 | public: 32 | constexpr CRC32Table() { 33 | for (uint32_t i = 0; i < 256; i++) { 34 | uint32_t r = i; 35 | r = ((r & 1) * Polynomial) ^ (r >> 1); 36 | r = ((r & 1) * Polynomial) ^ (r >> 1); 37 | r = ((r & 1) * Polynomial) ^ (r >> 1); 38 | r = ((r & 1) * Polynomial) ^ (r >> 1); 39 | r = ((r & 1) * Polynomial) ^ (r >> 1); 40 | r = ((r & 1) * Polynomial) ^ (r >> 1); 41 | r = ((r & 1) * Polynomial) ^ (r >> 1); 42 | r = ((r & 1) * Polynomial) ^ (r >> 1); 43 | table[i] = r; 44 | } 45 | for (size_t i = 256; i < table.size(); i++) { 46 | uint32_t value = table[i - 256]; 47 | table[i] = table[value & 0xff] ^ (value >> 8); 48 | } 49 | } 50 | 51 | constexpr uint32_t operator[](size_t index) const { 52 | return table[index]; 53 | } 54 | }; 55 | 56 | inline uint32_t getUint32LE(const std::byte* data) { 57 | return (uint32_t(data[0]) << 0) | (uint32_t(data[1]) << 8) | (uint32_t(data[2]) << 16) | 58 | (uint32_t(data[3]) << 24); 59 | } 60 | 61 | static constexpr CRC32Table<0xedb88320, 8> CRC32_TABLE; 62 | 63 | /** 64 | * Initialize a CRC32 to all 1 bits. 65 | */ 66 | static constexpr uint32_t CRC32_INIT = 0xffffffff; 67 | 68 | /** 69 | * Update a streaming CRC32 calculation. 70 | * 71 | * For performance, this implementation processes the data 8 bytes at a time, using the algorithm 72 | * presented at: https://github.com/komrad36/CRC#option-9-8-byte-tabular 73 | */ 74 | inline uint32_t crc32Update(const uint32_t prev, const std::byte* const data, const size_t length) { 75 | // Process bytes one by one until we reach the proper alignment. 76 | uint32_t r = prev; 77 | size_t offset = 0; 78 | for (; (uintptr_t(data + offset) & alignof(uint32_t)) != 0 && offset < length; offset++) { 79 | r = CRC32_TABLE[(r ^ uint8_t(data[offset])) & 0xff] ^ (r >> 8); 80 | } 81 | if (offset == length) { 82 | return r; 83 | } 84 | 85 | // Process 8 bytes (2 uint32s) at a time. 86 | size_t remainingBytes = length - offset; 87 | for (; remainingBytes >= 8; offset += 8, remainingBytes -= 8) { 88 | r ^= getUint32LE(data + offset); 89 | uint32_t r2 = getUint32LE(data + offset + 4); 90 | r = CRC32_TABLE[0 * 256 + ((r2 >> 24) & 0xff)] ^ CRC32_TABLE[1 * 256 + ((r2 >> 16) & 0xff)] ^ 91 | CRC32_TABLE[2 * 256 + ((r2 >> 8) & 0xff)] ^ CRC32_TABLE[3 * 256 + ((r2 >> 0) & 0xff)] ^ 92 | CRC32_TABLE[4 * 256 + ((r >> 24) & 0xff)] ^ CRC32_TABLE[5 * 256 + ((r >> 16) & 0xff)] ^ 93 | CRC32_TABLE[6 * 256 + ((r >> 8) & 0xff)] ^ CRC32_TABLE[7 * 256 + ((r >> 0) & 0xff)]; 94 | } 95 | 96 | // Process any remaining bytes one by one. 97 | for (; offset < length; offset++) { 98 | r = CRC32_TABLE[(r ^ uint8_t(data[offset])) & 0xff] ^ (r >> 8); 99 | } 100 | return r; 101 | } 102 | 103 | /** Finalize a CRC32 by inverting the output value. */ 104 | inline uint32_t crc32Final(uint32_t crc) { 105 | return crc ^ 0xffffffff; 106 | } 107 | 108 | } // namespace mcap::internal 109 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/include/mcap/errors.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace mcap { 6 | 7 | /** 8 | * @brief Status codes for MCAP readers and writers. 9 | */ 10 | enum class StatusCode { 11 | Success = 0, 12 | NotOpen, 13 | InvalidSchemaId, 14 | InvalidChannelId, 15 | FileTooSmall, 16 | ReadFailed, 17 | MagicMismatch, 18 | InvalidFile, 19 | InvalidRecord, 20 | InvalidOpCode, 21 | InvalidChunkOffset, 22 | InvalidFooter, 23 | DecompressionFailed, 24 | DecompressionSizeMismatch, 25 | UnrecognizedCompression, 26 | OpenFailed, 27 | MissingStatistics, 28 | InvalidMessageReadOptions, 29 | NoMessageIndexesAvailable, 30 | UnsupportedCompression, 31 | }; 32 | 33 | /** 34 | * @brief Wraps a status code and string message carrying additional context. 35 | */ 36 | struct [[nodiscard]] Status { 37 | StatusCode code; 38 | std::string message; 39 | 40 | Status() 41 | : code(StatusCode::Success) {} 42 | 43 | Status(StatusCode code) 44 | : code(code) { 45 | switch (code) { 46 | case StatusCode::Success: 47 | break; 48 | case StatusCode::NotOpen: 49 | message = "not open"; 50 | break; 51 | case StatusCode::InvalidSchemaId: 52 | message = "invalid schema id"; 53 | break; 54 | case StatusCode::InvalidChannelId: 55 | message = "invalid channel id"; 56 | break; 57 | case StatusCode::FileTooSmall: 58 | message = "file too small"; 59 | break; 60 | case StatusCode::ReadFailed: 61 | message = "read failed"; 62 | break; 63 | case StatusCode::MagicMismatch: 64 | message = "magic mismatch"; 65 | break; 66 | case StatusCode::InvalidFile: 67 | message = "invalid file"; 68 | break; 69 | case StatusCode::InvalidRecord: 70 | message = "invalid record"; 71 | break; 72 | case StatusCode::InvalidOpCode: 73 | message = "invalid opcode"; 74 | break; 75 | case StatusCode::InvalidChunkOffset: 76 | message = "invalid chunk offset"; 77 | break; 78 | case StatusCode::InvalidFooter: 79 | message = "invalid footer"; 80 | break; 81 | case StatusCode::DecompressionFailed: 82 | message = "decompression failed"; 83 | break; 84 | case StatusCode::DecompressionSizeMismatch: 85 | message = "decompression size mismatch"; 86 | break; 87 | case StatusCode::UnrecognizedCompression: 88 | message = "unrecognized compression"; 89 | break; 90 | case StatusCode::OpenFailed: 91 | message = "open failed"; 92 | break; 93 | case StatusCode::MissingStatistics: 94 | message = "missing statistics"; 95 | break; 96 | case StatusCode::InvalidMessageReadOptions: 97 | message = "message read options conflict"; 98 | break; 99 | case StatusCode::NoMessageIndexesAvailable: 100 | message = "file has no message indices"; 101 | break; 102 | case StatusCode::UnsupportedCompression: 103 | message = "unsupported compression"; 104 | break; 105 | default: 106 | message = "unknown"; 107 | break; 108 | } 109 | } 110 | 111 | Status(StatusCode code, const std::string& message) 112 | : code(code) 113 | , message(message) {} 114 | 115 | bool ok() const { 116 | return code == StatusCode::Success; 117 | } 118 | }; 119 | 120 | } // namespace mcap 121 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/include/mcap/internal.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "types.hpp" 4 | #include 5 | 6 | // Do not compile on systems with non-8-bit bytes 7 | static_assert(std::numeric_limits::digits == 8); 8 | 9 | namespace mcap { 10 | 11 | namespace internal { 12 | 13 | constexpr uint64_t MinHeaderLength = /* magic bytes */ sizeof(Magic) + 14 | /* opcode */ 1 + 15 | /* record length */ 8 + 16 | /* profile length */ 4 + 17 | /* library length */ 4; 18 | constexpr uint64_t FooterLength = /* opcode */ 1 + 19 | /* record length */ 8 + 20 | /* summary start */ 8 + 21 | /* summary offset start */ 8 + 22 | /* summary crc */ 4 + 23 | /* magic bytes */ sizeof(Magic); 24 | 25 | inline std::string ToHex(uint8_t byte) { 26 | std::string result{2, '\0'}; 27 | result[0] = "0123456789ABCDEF"[(uint8_t(byte) >> 4) & 0x0F]; 28 | result[1] = "0123456789ABCDEF"[uint8_t(byte) & 0x0F]; 29 | return result; 30 | } 31 | inline std::string ToHex(std::byte byte) { 32 | return ToHex(uint8_t(byte)); 33 | } 34 | 35 | inline std::string to_string(const std::string& arg) { 36 | return arg; 37 | } 38 | inline std::string to_string(std::string_view arg) { 39 | return std::string(arg); 40 | } 41 | inline std::string to_string(const char* arg) { 42 | return std::string(arg); 43 | } 44 | template 45 | [[nodiscard]] inline std::string StrCat(T&&... args) { 46 | using mcap::internal::to_string; 47 | using std::to_string; 48 | return ("" + ... + to_string(std::forward(args))); 49 | } 50 | 51 | inline uint32_t KeyValueMapSize(const KeyValueMap& map) { 52 | size_t size = 0; 53 | for (const auto& [key, value] : map) { 54 | size += 4 + key.size() + 4 + value.size(); 55 | } 56 | return (uint32_t)(size); 57 | } 58 | 59 | inline const std::string CompressionString(Compression compression) { 60 | switch (compression) { 61 | case Compression::None: 62 | default: 63 | return std::string{}; 64 | case Compression::Lz4: 65 | return "lz4"; 66 | case Compression::Zstd: 67 | return "zstd"; 68 | } 69 | } 70 | 71 | inline uint16_t ParseUint16(const std::byte* data) { 72 | return uint16_t(data[0]) | (uint16_t(data[1]) << 8); 73 | } 74 | 75 | inline uint32_t ParseUint32(const std::byte* data) { 76 | return uint32_t(data[0]) | (uint32_t(data[1]) << 8) | (uint32_t(data[2]) << 16) | 77 | (uint32_t(data[3]) << 24); 78 | } 79 | 80 | inline Status ParseUint32(const std::byte* data, uint64_t maxSize, uint32_t* output) { 81 | if (maxSize < 4) { 82 | const auto msg = StrCat("cannot read uint32 from ", maxSize, " bytes"); 83 | return Status{StatusCode::InvalidRecord, msg}; 84 | } 85 | *output = ParseUint32(data); 86 | return StatusCode::Success; 87 | } 88 | 89 | inline uint64_t ParseUint64(const std::byte* data) { 90 | return uint64_t(data[0]) | (uint64_t(data[1]) << 8) | (uint64_t(data[2]) << 16) | 91 | (uint64_t(data[3]) << 24) | (uint64_t(data[4]) << 32) | (uint64_t(data[5]) << 40) | 92 | (uint64_t(data[6]) << 48) | (uint64_t(data[7]) << 56); 93 | } 94 | 95 | inline Status ParseUint64(const std::byte* data, uint64_t maxSize, uint64_t* output) { 96 | if (maxSize < 8) { 97 | const auto msg = StrCat("cannot read uint64 from ", maxSize, " bytes"); 98 | return Status{StatusCode::InvalidRecord, msg}; 99 | } 100 | *output = ParseUint64(data); 101 | return StatusCode::Success; 102 | } 103 | 104 | inline Status ParseStringView(const std::byte* data, uint64_t maxSize, std::string_view* output) { 105 | uint32_t size = 0; 106 | if (auto status = ParseUint32(data, maxSize, &size); !status.ok()) { 107 | const auto msg = StrCat("cannot read string size: ", status.message); 108 | return Status{StatusCode::InvalidRecord, msg}; 109 | } 110 | if (uint64_t(size) > (maxSize - 4)) { 111 | const auto msg = StrCat("string size ", size, " exceeds remaining bytes ", (maxSize - 4)); 112 | return Status(StatusCode::InvalidRecord, msg); 113 | } 114 | *output = std::string_view(reinterpret_cast(data + 4), size); 115 | return StatusCode::Success; 116 | } 117 | 118 | inline Status ParseString(const std::byte* data, uint64_t maxSize, std::string* output) { 119 | uint32_t size = 0; 120 | if (auto status = ParseUint32(data, maxSize, &size); !status.ok()) { 121 | return status; 122 | } 123 | if (uint64_t(size) > (maxSize - 4)) { 124 | const auto msg = StrCat("string size ", size, " exceeds remaining bytes ", (maxSize - 4)); 125 | return Status(StatusCode::InvalidRecord, msg); 126 | } 127 | *output = std::string(reinterpret_cast(data + 4), size); 128 | return StatusCode::Success; 129 | } 130 | 131 | inline Status ParseByteArray(const std::byte* data, uint64_t maxSize, ByteArray* output) { 132 | uint32_t size = 0; 133 | if (auto status = ParseUint32(data, maxSize, &size); !status.ok()) { 134 | return status; 135 | } 136 | if (uint64_t(size) > (maxSize - 4)) { 137 | const auto msg = StrCat("byte array size ", size, " exceeds remaining bytes ", (maxSize - 4)); 138 | return Status(StatusCode::InvalidRecord, msg); 139 | } 140 | output->resize(size); 141 | std::memcpy(output->data(), data + 4, size); 142 | return StatusCode::Success; 143 | } 144 | 145 | inline Status ParseKeyValueMap(const std::byte* data, uint64_t maxSize, KeyValueMap* output) { 146 | uint32_t sizeInBytes = 0; 147 | if (auto status = ParseUint32(data, maxSize, &sizeInBytes); !status.ok()) { 148 | return status; 149 | } 150 | if (sizeInBytes > (maxSize - 4)) { 151 | const auto msg = 152 | StrCat("key-value map size ", sizeInBytes, " exceeds remaining bytes ", (maxSize - 4)); 153 | return Status(StatusCode::InvalidRecord, msg); 154 | } 155 | 156 | // Account for the byte size prefix in sizeInBytes to make the bounds checking 157 | // below simpler 158 | sizeInBytes += 4; 159 | 160 | output->clear(); 161 | uint64_t pos = 4; 162 | while (pos < sizeInBytes) { 163 | std::string_view key; 164 | if (auto status = ParseStringView(data + pos, sizeInBytes - pos, &key); !status.ok()) { 165 | const auto msg = StrCat("cannot read key-value map key at pos ", pos, ": ", status.message); 166 | return Status{StatusCode::InvalidRecord, msg}; 167 | } 168 | pos += 4 + key.size(); 169 | std::string_view value; 170 | if (auto status = ParseStringView(data + pos, sizeInBytes - pos, &value); !status.ok()) { 171 | const auto msg = StrCat("cannot read key-value map value for key \"", key, "\" at pos ", pos, 172 | ": ", status.message); 173 | return Status{StatusCode::InvalidRecord, msg}; 174 | } 175 | pos += 4 + value.size(); 176 | output->emplace(key, value); 177 | } 178 | return StatusCode::Success; 179 | } 180 | 181 | inline std::string MagicToHex(const std::byte* data) { 182 | return internal::ToHex(data[0]) + internal::ToHex(data[1]) + internal::ToHex(data[2]) + 183 | internal::ToHex(data[3]) + internal::ToHex(data[4]) + internal::ToHex(data[5]) + 184 | internal::ToHex(data[6]) + internal::ToHex(data[7]); 185 | } 186 | 187 | } // namespace internal 188 | 189 | } // namespace mcap 190 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/include/mcap/intervaltree.hpp: -------------------------------------------------------------------------------- 1 | // Adapted from 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace mcap::internal { 13 | 14 | template 15 | class Interval { 16 | public: 17 | Scalar start; 18 | Scalar stop; 19 | Value value; 20 | Interval(const Scalar& s, const Scalar& e, const Value& v) 21 | : start(std::min(s, e)) 22 | , stop(std::max(s, e)) 23 | , value(v) {} 24 | }; 25 | 26 | template 27 | Value intervalStart(const Interval& i) { 28 | return i.start; 29 | } 30 | 31 | template 32 | Value intervalStop(const Interval& i) { 33 | return i.stop; 34 | } 35 | 36 | template 37 | std::ostream& operator<<(std::ostream& out, const Interval& i) { 38 | out << "Interval(" << i.start << ", " << i.stop << "): " << i.value; 39 | return out; 40 | } 41 | 42 | template 43 | class IntervalTree { 44 | public: 45 | using interval = Interval; 46 | using interval_vector = std::vector; 47 | 48 | struct IntervalStartCmp { 49 | bool operator()(const interval& a, const interval& b) { 50 | return a.start < b.start; 51 | } 52 | }; 53 | 54 | struct IntervalStopCmp { 55 | bool operator()(const interval& a, const interval& b) { 56 | return a.stop < b.stop; 57 | } 58 | }; 59 | 60 | IntervalTree() 61 | : left(nullptr) 62 | , right(nullptr) 63 | , center(Scalar(0)) {} 64 | 65 | ~IntervalTree() = default; 66 | 67 | std::unique_ptr clone() const { 68 | return std::unique_ptr(new IntervalTree(*this)); 69 | } 70 | 71 | IntervalTree(const IntervalTree& other) 72 | : intervals(other.intervals) 73 | , left(other.left ? other.left->clone() : nullptr) 74 | , right(other.right ? other.right->clone() : nullptr) 75 | , center(other.center) {} 76 | 77 | IntervalTree& operator=(IntervalTree&&) = default; 78 | IntervalTree(IntervalTree&&) = default; 79 | 80 | IntervalTree& operator=(const IntervalTree& other) { 81 | center = other.center; 82 | intervals = other.intervals; 83 | left = other.left ? other.left->clone() : nullptr; 84 | right = other.right ? other.right->clone() : nullptr; 85 | return *this; 86 | } 87 | 88 | IntervalTree(interval_vector&& ivals, std::size_t depth = 16, std::size_t minbucket = 64, 89 | std::size_t maxbucket = 512, Scalar leftextent = 0, Scalar rightextent = 0) 90 | : left(nullptr) 91 | , right(nullptr) { 92 | --depth; 93 | const auto minmaxStop = std::minmax_element(ivals.begin(), ivals.end(), IntervalStopCmp()); 94 | const auto minmaxStart = std::minmax_element(ivals.begin(), ivals.end(), IntervalStartCmp()); 95 | if (!ivals.empty()) { 96 | center = (minmaxStart.first->start + minmaxStop.second->stop) / 2; 97 | } 98 | if (leftextent == 0 && rightextent == 0) { 99 | // sort intervals by start 100 | std::sort(ivals.begin(), ivals.end(), IntervalStartCmp()); 101 | } else { 102 | assert(std::is_sorted(ivals.begin(), ivals.end(), IntervalStartCmp())); 103 | } 104 | if (depth == 0 || (ivals.size() < minbucket && ivals.size() < maxbucket)) { 105 | std::sort(ivals.begin(), ivals.end(), IntervalStartCmp()); 106 | intervals = std::move(ivals); 107 | assert(is_valid().first); 108 | return; 109 | } else { 110 | Scalar leftp = 0; 111 | Scalar rightp = 0; 112 | 113 | if (leftextent || rightextent) { 114 | leftp = leftextent; 115 | rightp = rightextent; 116 | } else { 117 | leftp = ivals.front().start; 118 | rightp = std::max_element(ivals.begin(), ivals.end(), IntervalStopCmp())->stop; 119 | } 120 | 121 | interval_vector lefts; 122 | interval_vector rights; 123 | 124 | for (typename interval_vector::const_iterator i = ivals.begin(); i != ivals.end(); ++i) { 125 | const interval& interval = *i; 126 | if (interval.stop < center) { 127 | lefts.push_back(interval); 128 | } else if (interval.start > center) { 129 | rights.push_back(interval); 130 | } else { 131 | assert(interval.start <= center); 132 | assert(center <= interval.stop); 133 | intervals.push_back(interval); 134 | } 135 | } 136 | 137 | if (!lefts.empty()) { 138 | left.reset(new IntervalTree(std::move(lefts), depth, minbucket, maxbucket, leftp, center)); 139 | } 140 | if (!rights.empty()) { 141 | right.reset( 142 | new IntervalTree(std::move(rights), depth, minbucket, maxbucket, center, rightp)); 143 | } 144 | } 145 | assert(is_valid().first); 146 | } 147 | 148 | // Call f on all intervals near the range [start, stop]: 149 | template 150 | void visit_near(const Scalar& start, const Scalar& stop, UnaryFunction f) const { 151 | if (!intervals.empty() && !(stop < intervals.front().start)) { 152 | for (auto& i : intervals) { 153 | f(i); 154 | } 155 | } 156 | if (left && start <= center) { 157 | left->visit_near(start, stop, f); 158 | } 159 | if (right && stop >= center) { 160 | right->visit_near(start, stop, f); 161 | } 162 | } 163 | 164 | // Call f on all intervals crossing pos 165 | template 166 | void visit_overlapping(const Scalar& pos, UnaryFunction f) const { 167 | visit_overlapping(pos, pos, f); 168 | } 169 | 170 | // Call f on all intervals overlapping [start, stop] 171 | template 172 | void visit_overlapping(const Scalar& start, const Scalar& stop, UnaryFunction f) const { 173 | auto filterF = [&](const interval& interval) { 174 | if (interval.stop >= start && interval.start <= stop) { 175 | // Only apply f if overlapping 176 | f(interval); 177 | } 178 | }; 179 | visit_near(start, stop, filterF); 180 | } 181 | 182 | // Call f on all intervals contained within [start, stop] 183 | template 184 | void visit_contained(const Scalar& start, const Scalar& stop, UnaryFunction f) const { 185 | auto filterF = [&](const interval& interval) { 186 | if (start <= interval.start && interval.stop <= stop) { 187 | f(interval); 188 | } 189 | }; 190 | visit_near(start, stop, filterF); 191 | } 192 | 193 | interval_vector find_overlapping(const Scalar& start, const Scalar& stop) const { 194 | interval_vector result; 195 | visit_overlapping(start, stop, [&](const interval& interval) { 196 | result.emplace_back(interval); 197 | }); 198 | return result; 199 | } 200 | 201 | interval_vector find_contained(const Scalar& start, const Scalar& stop) const { 202 | interval_vector result; 203 | visit_contained(start, stop, [&](const interval& interval) { 204 | result.push_back(interval); 205 | }); 206 | return result; 207 | } 208 | 209 | bool empty() const { 210 | if (left && !left->empty()) { 211 | return false; 212 | } 213 | if (!intervals.empty()) { 214 | return false; 215 | } 216 | if (right && !right->empty()) { 217 | return false; 218 | } 219 | return true; 220 | } 221 | 222 | template 223 | void visit_all(UnaryFunction f) const { 224 | if (left) { 225 | left->visit_all(f); 226 | } 227 | std::for_each(intervals.begin(), intervals.end(), f); 228 | if (right) { 229 | right->visit_all(f); 230 | } 231 | } 232 | 233 | std::pair extent() const { 234 | struct Extent { 235 | std::pair x{std::numeric_limits::max(), 236 | std::numeric_limits::min()}; 237 | void operator()(const interval& interval) { 238 | x.first = std::min(x.first, interval.start); 239 | x.second = std::max(x.second, interval.stop); 240 | } 241 | }; 242 | Extent extent; 243 | 244 | visit_all([&](const interval& interval) { 245 | extent(interval); 246 | }); 247 | return extent.x; 248 | } 249 | 250 | // Check all constraints. 251 | // If first is false, second is invalid. 252 | std::pair> is_valid() const { 253 | const auto minmaxStop = 254 | std::minmax_element(intervals.begin(), intervals.end(), IntervalStopCmp()); 255 | const auto minmaxStart = 256 | std::minmax_element(intervals.begin(), intervals.end(), IntervalStartCmp()); 257 | 258 | std::pair> result = { 259 | true, {std::numeric_limits::max(), std::numeric_limits::min()}}; 260 | if (!intervals.empty()) { 261 | result.second.first = std::min(result.second.first, minmaxStart.first->start); 262 | result.second.second = std::min(result.second.second, minmaxStop.second->stop); 263 | } 264 | if (left) { 265 | auto valid = left->is_valid(); 266 | result.first &= valid.first; 267 | result.second.first = std::min(result.second.first, valid.second.first); 268 | result.second.second = std::min(result.second.second, valid.second.second); 269 | if (!result.first) { 270 | return result; 271 | } 272 | if (valid.second.second >= center) { 273 | result.first = false; 274 | return result; 275 | } 276 | } 277 | if (right) { 278 | auto valid = right->is_valid(); 279 | result.first &= valid.first; 280 | result.second.first = std::min(result.second.first, valid.second.first); 281 | result.second.second = std::min(result.second.second, valid.second.second); 282 | if (!result.first) { 283 | return result; 284 | } 285 | if (valid.second.first <= center) { 286 | result.first = false; 287 | return result; 288 | } 289 | } 290 | if (!std::is_sorted(intervals.begin(), intervals.end(), IntervalStartCmp())) { 291 | result.first = false; 292 | } 293 | return result; 294 | } 295 | 296 | private: 297 | interval_vector intervals; 298 | std::unique_ptr left; 299 | std::unique_ptr right; 300 | Scalar center; 301 | }; 302 | 303 | } // namespace mcap::internal 304 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/include/mcap/mcap.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "reader.hpp" 4 | #include "writer.hpp" 5 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/include/mcap/read_job_queue.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "types.hpp" 4 | #include 5 | #include 6 | 7 | namespace mcap::internal { 8 | 9 | // Helper for writing compile-time exhaustive variant visitors. 10 | template 11 | inline constexpr bool always_false_v = false; 12 | 13 | /** 14 | * @brief A job to read a specific message at offset `offset` from the decompressed chunk 15 | * stored in `chunkReaderIndex`. A timestamp is provided to order this job relative to other jobs. 16 | */ 17 | struct ReadMessageJob { 18 | Timestamp timestamp; 19 | RecordOffset offset; 20 | size_t chunkReaderIndex; 21 | }; 22 | 23 | /** 24 | * @brief A job to decompress the chunk starting at `chunkStartOffset`. The message indices 25 | * starting directly after the chunk record and ending at `messageIndexEndOffset` will be used to 26 | * find specific messages within the chunk. 27 | */ 28 | struct DecompressChunkJob { 29 | Timestamp messageStartTime; 30 | Timestamp messageEndTime; 31 | ByteOffset chunkStartOffset; 32 | ByteOffset messageIndexEndOffset; 33 | }; 34 | 35 | /** 36 | * @brief A union of jobs that an indexed MCAP reader executes. 37 | */ 38 | using ReadJob = std::variant; 39 | 40 | /** 41 | * @brief A priority queue of jobs for an indexed MCAP reader to execute. 42 | */ 43 | struct ReadJobQueue { 44 | private: 45 | bool reverse_ = false; 46 | std::vector heap_; 47 | 48 | /** 49 | * @brief return the timestamp key that should be used to compare jobs. 50 | */ 51 | static Timestamp TimeComparisonKey(const ReadJob& job, bool reverse) { 52 | Timestamp result = 0; 53 | std::visit( 54 | [&](auto&& arg) { 55 | using T = std::decay_t; 56 | if constexpr (std::is_same_v) { 57 | result = arg.timestamp; 58 | } else if constexpr (std::is_same_v) { 59 | if (reverse) { 60 | result = arg.messageEndTime; 61 | } else { 62 | result = arg.messageStartTime; 63 | } 64 | } else { 65 | static_assert(always_false_v, "non-exhaustive visitor!"); 66 | } 67 | }, 68 | job); 69 | return result; 70 | } 71 | static RecordOffset PositionComparisonKey(const ReadJob& job, bool reverse) { 72 | RecordOffset result; 73 | std::visit( 74 | [&](auto&& arg) { 75 | using T = std::decay_t; 76 | if constexpr (std::is_same_v) { 77 | result = arg.offset; 78 | } else if constexpr (std::is_same_v) { 79 | if (reverse) { 80 | result.offset = arg.messageIndexEndOffset; 81 | } else { 82 | result.offset = arg.chunkStartOffset; 83 | } 84 | } else { 85 | static_assert(always_false_v, "non-exhaustive visitor!"); 86 | } 87 | }, 88 | job); 89 | return result; 90 | } 91 | 92 | static bool CompareForward(const ReadJob& a, const ReadJob& b) { 93 | auto aTimestamp = TimeComparisonKey(a, false); 94 | auto bTimestamp = TimeComparisonKey(b, false); 95 | if (aTimestamp == bTimestamp) { 96 | return PositionComparisonKey(a, false) > PositionComparisonKey(b, false); 97 | } 98 | return aTimestamp > bTimestamp; 99 | } 100 | 101 | static bool CompareReverse(const ReadJob& a, const ReadJob& b) { 102 | auto aTimestamp = TimeComparisonKey(a, true); 103 | auto bTimestamp = TimeComparisonKey(b, true); 104 | if (aTimestamp == bTimestamp) { 105 | return PositionComparisonKey(a, true) < PositionComparisonKey(b, true); 106 | } 107 | return aTimestamp < bTimestamp; 108 | } 109 | 110 | public: 111 | explicit ReadJobQueue(bool reverse) 112 | : reverse_(reverse) {} 113 | void push(DecompressChunkJob&& decompressChunkJob) { 114 | heap_.emplace_back(std::move(decompressChunkJob)); 115 | if (!reverse_) { 116 | std::push_heap(heap_.begin(), heap_.end(), CompareForward); 117 | } else { 118 | std::push_heap(heap_.begin(), heap_.end(), CompareReverse); 119 | } 120 | } 121 | 122 | void push(ReadMessageJob&& readMessageJob) { 123 | heap_.emplace_back(std::move(readMessageJob)); 124 | if (!reverse_) { 125 | std::push_heap(heap_.begin(), heap_.end(), CompareForward); 126 | } else { 127 | std::push_heap(heap_.begin(), heap_.end(), CompareReverse); 128 | } 129 | } 130 | 131 | ReadJob pop() { 132 | if (!reverse_) { 133 | std::pop_heap(heap_.begin(), heap_.end(), CompareForward); 134 | } else { 135 | std::pop_heap(heap_.begin(), heap_.end(), CompareReverse); 136 | } 137 | auto popped = heap_.back(); 138 | heap_.pop_back(); 139 | return popped; 140 | } 141 | 142 | size_t len() const { 143 | return heap_.size(); 144 | } 145 | }; 146 | 147 | } // namespace mcap::internal 148 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/include/mcap/types.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "errors.hpp" 4 | #include "visibility.hpp" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace mcap { 15 | 16 | #define MCAP_LIBRARY_VERSION "1.3.0" 17 | 18 | using SchemaId = uint16_t; 19 | using ChannelId = uint16_t; 20 | using Timestamp = uint64_t; 21 | using ByteOffset = uint64_t; 22 | using KeyValueMap = std::unordered_map; 23 | using ByteArray = std::vector; 24 | using ProblemCallback = std::function; 25 | 26 | constexpr char SpecVersion = '0'; 27 | constexpr char LibraryVersion[] = MCAP_LIBRARY_VERSION; 28 | constexpr uint8_t Magic[] = {137, 77, 67, 65, 80, SpecVersion, 13, 10}; // "\x89MCAP0\r\n" 29 | constexpr uint64_t DefaultChunkSize = 1024 * 768; 30 | constexpr ByteOffset EndOffset = std::numeric_limits::max(); 31 | constexpr Timestamp MaxTime = std::numeric_limits::max(); 32 | 33 | /** 34 | * @brief Supported MCAP compression algorithms. 35 | */ 36 | enum struct Compression { 37 | None, 38 | Lz4, 39 | Zstd, 40 | }; 41 | 42 | /** 43 | * @brief Compression level to use when compression is enabled. Slower generally 44 | * produces smaller files, at the expense of more CPU time. These levels map to 45 | * different internal settings for each compression algorithm. 46 | */ 47 | enum struct CompressionLevel { 48 | Fastest, 49 | Fast, 50 | Default, 51 | Slow, 52 | Slowest, 53 | }; 54 | 55 | /** 56 | * @brief MCAP record types. 57 | */ 58 | enum struct OpCode : uint8_t { 59 | Header = 0x01, 60 | Footer = 0x02, 61 | Schema = 0x03, 62 | Channel = 0x04, 63 | Message = 0x05, 64 | Chunk = 0x06, 65 | MessageIndex = 0x07, 66 | ChunkIndex = 0x08, 67 | Attachment = 0x09, 68 | AttachmentIndex = 0x0A, 69 | Statistics = 0x0B, 70 | Metadata = 0x0C, 71 | MetadataIndex = 0x0D, 72 | SummaryOffset = 0x0E, 73 | DataEnd = 0x0F, 74 | }; 75 | 76 | /** 77 | * @brief Get the string representation of an OpCode. 78 | */ 79 | MCAP_PUBLIC 80 | constexpr std::string_view OpCodeString(OpCode opcode); 81 | 82 | /** 83 | * @brief A generic Type-Length-Value record using a uint8 type and uint64 84 | * length. This is the generic form of all MCAP records. 85 | */ 86 | struct MCAP_PUBLIC Record { 87 | OpCode opcode; 88 | uint64_t dataSize; 89 | std::byte* data; 90 | 91 | uint64_t recordSize() const { 92 | return sizeof(opcode) + sizeof(dataSize) + dataSize; 93 | } 94 | }; 95 | 96 | /** 97 | * @brief Appears at the beginning of every MCAP file (after the magic byte 98 | * sequence) and contains the recording profile (see 99 | * ) and 100 | * a string signature of the recording library. 101 | */ 102 | struct MCAP_PUBLIC Header { 103 | std::string profile; 104 | std::string library; 105 | }; 106 | 107 | /** 108 | * @brief The final record in an MCAP file (before the trailing magic byte 109 | * sequence). Contains byte offsets from the start of the file to the Summary 110 | * and Summary Offset sections, along with an optional CRC of the combined 111 | * Summary and Summary Offset sections. A `summaryStart` and 112 | * `summaryOffsetStart` of zero indicates no Summary section is available. 113 | */ 114 | struct MCAP_PUBLIC Footer { 115 | ByteOffset summaryStart; 116 | ByteOffset summaryOffsetStart; 117 | uint32_t summaryCrc; 118 | 119 | Footer() = default; 120 | Footer(ByteOffset summaryStart, ByteOffset summaryOffsetStart) 121 | : summaryStart(summaryStart) 122 | , summaryOffsetStart(summaryOffsetStart) 123 | , summaryCrc(0) {} 124 | }; 125 | 126 | /** 127 | * @brief Describes a schema used for message encoding and decoding and/or 128 | * describing the shape of messages. One or more Channel records map to a single 129 | * Schema. 130 | */ 131 | struct MCAP_PUBLIC Schema { 132 | SchemaId id; 133 | std::string name; 134 | std::string encoding; 135 | ByteArray data; 136 | 137 | Schema() = default; 138 | 139 | Schema(const std::string_view name, const std::string_view encoding, const std::string_view data) 140 | : name(name) 141 | , encoding(encoding) 142 | , data{reinterpret_cast(data.data()), 143 | reinterpret_cast(data.data() + data.size())} {} 144 | 145 | Schema(const std::string_view name, const std::string_view encoding, const ByteArray& data) 146 | : name(name) 147 | , encoding(encoding) 148 | , data{data} {} 149 | }; 150 | 151 | /** 152 | * @brief Describes a Channel that messages are written to. A Channel represents 153 | * a single connection from a publisher to a topic, so each topic will have one 154 | * Channel per publisher. Channels optionally reference a Schema, for message 155 | * encodings that are not self-describing (e.g. JSON) or when schema information 156 | * is available (e.g. JSONSchema). 157 | */ 158 | struct MCAP_PUBLIC Channel { 159 | ChannelId id; 160 | std::string topic; 161 | std::string messageEncoding; 162 | SchemaId schemaId; 163 | KeyValueMap metadata; 164 | 165 | Channel() = default; 166 | 167 | Channel(const std::string_view topic, const std::string_view messageEncoding, SchemaId schemaId, 168 | const KeyValueMap& metadata = {}) 169 | : topic(topic) 170 | , messageEncoding(messageEncoding) 171 | , schemaId(schemaId) 172 | , metadata(metadata) {} 173 | }; 174 | 175 | using SchemaPtr = std::shared_ptr; 176 | using ChannelPtr = std::shared_ptr; 177 | 178 | /** 179 | * @brief A single Message published to a Channel. 180 | */ 181 | struct MCAP_PUBLIC Message { 182 | ChannelId channelId; 183 | /** 184 | * @brief An optional sequence number. If non-zero, sequence numbers should be 185 | * unique per channel and increasing over time. 186 | */ 187 | uint32_t sequence; 188 | /** 189 | * @brief Nanosecond timestamp when this message was recorded or received for 190 | * recording. 191 | */ 192 | Timestamp logTime; 193 | /** 194 | * @brief Nanosecond timestamp when this message was initially published. If 195 | * not available, this should be set to `logTime`. 196 | */ 197 | Timestamp publishTime; 198 | /** 199 | * @brief Size of the message payload in bytes, pointed to via `data`. 200 | */ 201 | uint64_t dataSize; 202 | /** 203 | * @brief A pointer to the message payload. For readers, this pointer is only 204 | * valid for the lifetime of an onMessage callback or before the message 205 | * iterator is advanced. 206 | */ 207 | const std::byte* data = nullptr; 208 | }; 209 | 210 | /** 211 | * @brief An collection of Schemas, Channels, and Messages that supports 212 | * compression and indexing. 213 | */ 214 | struct MCAP_PUBLIC Chunk { 215 | Timestamp messageStartTime; 216 | Timestamp messageEndTime; 217 | ByteOffset uncompressedSize; 218 | uint32_t uncompressedCrc; 219 | std::string compression; 220 | ByteOffset compressedSize; 221 | const std::byte* records = nullptr; 222 | }; 223 | 224 | /** 225 | * @brief A list of timestamps to byte offsets for a single Channel. This record 226 | * appears after each Chunk, one per Channel that appeared in that Chunk. 227 | */ 228 | struct MCAP_PUBLIC MessageIndex { 229 | ChannelId channelId; 230 | std::vector> records; 231 | }; 232 | 233 | /** 234 | * @brief Chunk Index records are found in the Summary section, providing 235 | * summary information for a single Chunk and pointing to each Message Index 236 | * record associated with that Chunk. 237 | */ 238 | struct MCAP_PUBLIC ChunkIndex { 239 | Timestamp messageStartTime; 240 | Timestamp messageEndTime; 241 | ByteOffset chunkStartOffset; 242 | ByteOffset chunkLength; 243 | std::unordered_map messageIndexOffsets; 244 | ByteOffset messageIndexLength; 245 | std::string compression; 246 | ByteOffset compressedSize; 247 | ByteOffset uncompressedSize; 248 | }; 249 | 250 | /** 251 | * @brief An Attachment is an arbitrary file embedded in an MCAP file, including 252 | * a name, media type, timestamps, and optional CRC. Attachment records are 253 | * written in the Data section, outside of Chunks. 254 | */ 255 | struct MCAP_PUBLIC Attachment { 256 | Timestamp logTime; 257 | Timestamp createTime; 258 | std::string name; 259 | std::string mediaType; 260 | uint64_t dataSize; 261 | const std::byte* data = nullptr; 262 | uint32_t crc; 263 | }; 264 | 265 | /** 266 | * @brief Attachment Index records are found in the Summary section, providing 267 | * summary information for a single Attachment. 268 | */ 269 | struct MCAP_PUBLIC AttachmentIndex { 270 | ByteOffset offset; 271 | ByteOffset length; 272 | Timestamp logTime; 273 | Timestamp createTime; 274 | uint64_t dataSize; 275 | std::string name; 276 | std::string mediaType; 277 | 278 | AttachmentIndex() = default; 279 | AttachmentIndex(const Attachment& attachment, ByteOffset fileOffset) 280 | : offset(fileOffset) 281 | , length(9 + 282 | /* name */ 4 + attachment.name.size() + 283 | /* log_time */ 8 + 284 | /* create_time */ 8 + 285 | /* media_type */ 4 + attachment.mediaType.size() + 286 | /* data */ 8 + attachment.dataSize + 287 | /* crc */ 4) 288 | , logTime(attachment.logTime) 289 | , createTime(attachment.createTime) 290 | , dataSize(attachment.dataSize) 291 | , name(attachment.name) 292 | , mediaType(attachment.mediaType) {} 293 | }; 294 | 295 | /** 296 | * @brief The Statistics record is found in the Summary section, providing 297 | * counts and timestamp ranges for the entire file. 298 | */ 299 | struct MCAP_PUBLIC Statistics { 300 | uint64_t messageCount; 301 | uint16_t schemaCount; 302 | uint32_t channelCount; 303 | uint32_t attachmentCount; 304 | uint32_t metadataCount; 305 | uint32_t chunkCount; 306 | Timestamp messageStartTime; 307 | Timestamp messageEndTime; 308 | std::unordered_map channelMessageCounts; 309 | }; 310 | 311 | /** 312 | * @brief Holds a named map of key/value strings containing arbitrary user data. 313 | * Metadata records are found in the Data section, outside of Chunks. 314 | */ 315 | struct MCAP_PUBLIC Metadata { 316 | std::string name; 317 | KeyValueMap metadata; 318 | }; 319 | 320 | /** 321 | * @brief Metadata Index records are found in the Summary section, providing 322 | * summary information for a single Metadata record. 323 | */ 324 | struct MCAP_PUBLIC MetadataIndex { 325 | uint64_t offset; 326 | uint64_t length; 327 | std::string name; 328 | 329 | MetadataIndex() = default; 330 | MetadataIndex(const Metadata& metadata, ByteOffset fileOffset); 331 | }; 332 | 333 | /** 334 | * @brief Summary Offset records are found in the Summary Offset section. 335 | * Records in the Summary section are grouped together, and for each record type 336 | * found in the Summary section, a Summary Offset references the file offset and 337 | * length where that type of Summary record can be found. 338 | */ 339 | struct MCAP_PUBLIC SummaryOffset { 340 | OpCode groupOpCode; 341 | ByteOffset groupStart; 342 | ByteOffset groupLength; 343 | }; 344 | 345 | /** 346 | * @brief The final record in the Data section, signaling the end of Data and 347 | * beginning of Summary. Optionally contains a CRC of the entire Data section. 348 | */ 349 | struct MCAP_PUBLIC DataEnd { 350 | uint32_t dataSectionCrc; 351 | }; 352 | 353 | struct MCAP_PUBLIC RecordOffset { 354 | ByteOffset offset; 355 | std::optional chunkOffset; 356 | 357 | RecordOffset() = default; 358 | explicit RecordOffset(ByteOffset offset_) 359 | : offset(offset_) {} 360 | RecordOffset(ByteOffset offset_, ByteOffset chunkOffset_) 361 | : offset(offset_) 362 | , chunkOffset(chunkOffset_) {} 363 | 364 | bool operator==(const RecordOffset& other) const; 365 | bool operator>(const RecordOffset& other) const; 366 | 367 | bool operator!=(const RecordOffset& other) const { 368 | return !(*this == other); 369 | } 370 | bool operator>=(const RecordOffset& other) const { 371 | return ((*this == other) || (*this > other)); 372 | } 373 | bool operator<(const RecordOffset& other) const { 374 | return !(*this >= other); 375 | } 376 | bool operator<=(const RecordOffset& other) const { 377 | return !(*this > other); 378 | } 379 | }; 380 | 381 | /** 382 | * @brief Returned when iterating over Messages in a file, MessageView contains 383 | * a reference to one Message, a pointer to its Channel, and an optional pointer 384 | * to that Channel's Schema. The Channel pointer is guaranteed to be valid, 385 | * while the Schema pointer may be null if the Channel references schema_id 0. 386 | */ 387 | struct MCAP_PUBLIC MessageView { 388 | const Message& message; 389 | const ChannelPtr channel; 390 | const SchemaPtr schema; 391 | const RecordOffset messageOffset; 392 | 393 | MessageView(const Message& message, const ChannelPtr channel, const SchemaPtr schema, 394 | RecordOffset offset) 395 | : message(message) 396 | , channel(channel) 397 | , schema(schema) 398 | , messageOffset(offset) {} 399 | }; 400 | 401 | } // namespace mcap 402 | 403 | #ifdef MCAP_IMPLEMENTATION 404 | # include "types.inl" 405 | #endif 406 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/include/mcap/types.inl: -------------------------------------------------------------------------------- 1 | #include "internal.hpp" 2 | 3 | namespace mcap { 4 | 5 | constexpr std::string_view OpCodeString(OpCode opcode) { 6 | switch (opcode) { 7 | case OpCode::Header: 8 | return "Header"; 9 | case OpCode::Footer: 10 | return "Footer"; 11 | case OpCode::Schema: 12 | return "Schema"; 13 | case OpCode::Channel: 14 | return "Channel"; 15 | case OpCode::Message: 16 | return "Message"; 17 | case OpCode::Chunk: 18 | return "Chunk"; 19 | case OpCode::MessageIndex: 20 | return "MessageIndex"; 21 | case OpCode::ChunkIndex: 22 | return "ChunkIndex"; 23 | case OpCode::Attachment: 24 | return "Attachment"; 25 | case OpCode::AttachmentIndex: 26 | return "AttachmentIndex"; 27 | case OpCode::Statistics: 28 | return "Statistics"; 29 | case OpCode::Metadata: 30 | return "Metadata"; 31 | case OpCode::MetadataIndex: 32 | return "MetadataIndex"; 33 | case OpCode::SummaryOffset: 34 | return "SummaryOffset"; 35 | case OpCode::DataEnd: 36 | return "DataEnd"; 37 | default: 38 | return "Unknown"; 39 | } 40 | } 41 | 42 | MetadataIndex::MetadataIndex(const Metadata& metadata, ByteOffset fileOffset) 43 | : offset(fileOffset) 44 | , length(9 + 4 + metadata.name.size() + 4 + internal::KeyValueMapSize(metadata.metadata)) 45 | , name(metadata.name) {} 46 | 47 | bool RecordOffset::operator==(const RecordOffset& other) const { 48 | if (chunkOffset != std::nullopt && other.chunkOffset != std::nullopt) { 49 | if (*chunkOffset != *other.chunkOffset) { 50 | // messages are in separate chunks, cannot be equal. 51 | return false; 52 | } 53 | // messages are in the same chunk, compare chunk-level offsets. 54 | return (offset == other.offset); 55 | } 56 | if (chunkOffset != std::nullopt || other.chunkOffset != std::nullopt) { 57 | // one message is in a chunk and one is not, cannot be equal. 58 | return false; 59 | } 60 | // neither message is in a chunk, compare file-level offsets. 61 | return (offset == other.offset); 62 | } 63 | 64 | bool RecordOffset::operator>(const RecordOffset& other) const { 65 | if (chunkOffset != std::nullopt) { 66 | if (other.chunkOffset != std::nullopt) { 67 | if (*chunkOffset == *other.chunkOffset) { 68 | // messages are in the same chunk, compare chunk-level offsets. 69 | return (offset > other.offset); 70 | } 71 | // messages are in separate chunks, compare file-level offsets 72 | return (*chunkOffset > *other.chunkOffset); 73 | } else { 74 | // this message is in a chunk, other is not, compare file-level offsets. 75 | return (*chunkOffset > other.offset); 76 | } 77 | } 78 | if (other.chunkOffset != std::nullopt) { 79 | // other message is in a chunk, this is not, compare file-level offsets. 80 | return (offset > *other.chunkOffset); 81 | } 82 | // neither message is in a chunk, compare file-level offsets. 83 | return (offset > other.offset); 84 | } 85 | 86 | } // namespace mcap 87 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/include/mcap/visibility.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined _WIN32 || defined __CYGWIN__ 4 | # ifdef __GNUC__ 5 | # define MCAP_EXPORT __attribute__((dllexport)) 6 | # define MCAP_IMPORT __attribute__((dllimport)) 7 | # else 8 | # define MCAP_EXPORT __declspec(dllexport) 9 | # define MCAP_IMPORT __declspec(dllimport) 10 | # endif 11 | # ifdef MCAP_IMPLEMENTATION 12 | # define MCAP_PUBLIC MCAP_EXPORT 13 | # else 14 | # define MCAP_PUBLIC MCAP_IMPORT 15 | # endif 16 | #else 17 | # define MCAP_EXPORT __attribute__((visibility("default"))) 18 | # define MCAP_IMPORT 19 | # if __GNUC__ >= 4 20 | # define MCAP_PUBLIC __attribute__((visibility("default"))) 21 | # else 22 | # define MCAP_PUBLIC 23 | # endif 24 | #endif 25 | -------------------------------------------------------------------------------- /data_tamer_cpp/3rdparty/mcap/mcap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | -------------------------------------------------------------------------------- /data_tamer_cpp/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package data_tamer 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.3 (2025-05-23) 6 | ----------- 7 | * Remove ament_target_dependencies usage 8 | 9 | 1.0.2 (2025-05-12) 10 | ----------- 11 | * Merge pull request `#52 `_ from PickNikRobotics/fix_build_farm_3 12 | Try installing test deps no matter what 13 | * try installing test deps no matter what 14 | * Merge pull request `#51 `_ from PickNikRobotics/fix_build_farm_2 15 | ignore build ros argument in test CMake 16 | * ignore build ros argument in test 17 | * Merge pull request `#50 `_ from PickNikRobotics/fix_build_farm 18 | Fix ament GTest usage 19 | * use ament add gtest 20 | * Merge pull request `#49 `_ from PickNikRobotics/fix_gtest_link 21 | Get gtest from vendor on ROS 22 | * add myself to maintainer list, add package xml scheme 23 | * get gtest from vendor on ROS 24 | * Contributors: Henry Moore 25 | 26 | 1.0.1 (2025-03-03) 27 | ------------------ 28 | * force the size of BasicType ot be 1 byte (`#47 `_) 29 | * force the size of BasicType ot be 1 byte 30 | This fix the issue when the log is generated on two different computers 31 | using different compilers (example Linux / QNX) 32 | * fix 33 | * add file reset capabilities (`#37 `_) 34 | * Enable users to build without ROS (`#36 `_) 35 | * make building with ROS an option 36 | * clarify warnings, allow for default-building without ROS 37 | * switch name of ros build flag 38 | * add build ros argument for examples 39 | * fix incorrect message 40 | * Merge pull request `#32 `_ from PickNikRobotics/default_increment_filename 41 | Allow endless recording 42 | * remove unused variable 43 | * allow for unlimited recording 44 | * use a new mutex wrapper API (`#27 `_) 45 | * use a new mutx wrapper API 46 | * fix compilation 47 | * change docstrings to reference non-deprecated function 48 | * add now-missing includes 49 | --------- 50 | Co-authored-by: Henry Moore 51 | * Locked ptr test, documentation, and example (`#24 `_) 52 | * add locked ptr usage to example 53 | * update comment relating to locked ptr 54 | * remove unused headers 55 | * add test for locked ptr and non-blocking method 56 | * remove unsafe lockedPtr get function 57 | * Merge pull request `#23 `_ from torsoelectronics/main 58 | Fix compile error 59 | * Fix build error 60 | * Contributors: Daniel Mouritzen, Davide Faconti, Henry Moore 61 | 62 | 1.0.0 (2024-04-30) 63 | ------------------ 64 | * Support lifecycle node for ros2 publisher sink (`#17 `_) 65 | * Support lifecycle node for ros2 publisher sink 66 | * Remove unused member variable node\_ 67 | * Add template for both constructors 68 | * more efficient locking of LoggedValue and new clang format 69 | * refactoring custom types 70 | * fix compilation with and without conan 71 | * new clang format 72 | * add mcap to 3rdparty 73 | * Contributors: Davide Faconti, Victor Massagué Respall 74 | 75 | 0.9.4 (2024-02-02) 76 | ------------------ 77 | * changed the way registerValue throws if you try registering the same address again 78 | * add unit tests to verify that vectors with changing size are OK 79 | * Contributors: Davide Faconti 80 | 81 | 0.9.3 (2024-02-01) 82 | ------------------ 83 | * add std::hash 84 | * fix dead-lock 85 | * Contributors: Davide Faconti 86 | 87 | 0.9.2 (2024-01-30) 88 | ------------------ 89 | * fix compilation in ament 90 | * Update CMakeLists.txt. Fix `#11 `_ 91 | * Contributors: Davide Faconti 92 | 93 | 0.9.1 (2024-01-12) 94 | ------------------ 95 | * add support for enums 96 | * renamed folder to data_tamer_cpp 97 | * Contributors: Davide Faconti 98 | 99 | 0.8.0 (2023-11-30) 100 | ------------------ 101 | * API change related to CustomSerializers 102 | * Contributors: Davide Faconti 103 | 104 | 0.7.0 (2023-11-28) 105 | ------------------ 106 | * recursive_mutex and call it a day 107 | * add MCAP option 108 | * add MCAPSink::stopRecording 109 | * add more types to mcap example 110 | * add ChannelsRegistry::clear() 111 | * extended tests 112 | * bug fixes and more tests 113 | * fix warning 114 | * compute fixed size at compilation time 115 | * new wrappying of TypeDefinition 116 | * refactoring type registry 117 | * major refactoring of custom types 118 | * Contributors: Davide Faconti 119 | 120 | 0.6.0 (2023-11-23) 121 | ------------------ 122 | @ add back compatibility to data_tamer_parser 123 | * works correctly with plotjuggler 124 | * fix ROS2 compilation 125 | * Contributors: Davide Faconti 126 | 127 | 0.5.0 (2023-11-22) 128 | ------------------ 129 | * preliminary custom type support 130 | * Contributors: Davide Faconti 131 | 132 | 0.4.1 (2023-11-21) 133 | ------------------ 134 | 135 | 0.4.0 (2023-11-21) 136 | ------------------ 137 | * add again channel name to hash 138 | * bug fixes in schema hash and parsing 139 | * add benchmark 140 | * readme update 141 | * added data_tamer_parser with some samples and testing 142 | * add locked reference 143 | * bug fixes and tests 144 | * refactored API to support containers 145 | * Contributors: Davide Faconti 146 | 147 | 0.3.0 (2023-11-14) 148 | ------------------ 149 | * add coverage 150 | * fix bug 151 | * add CI 152 | * unit test added 153 | * allow registering again with new pointer 154 | * add docs 155 | * use custom mutex on linux 156 | * adding ros2 example 157 | * ros2 publisher sink 158 | * Contributors: Davide Faconti 159 | 160 | 0.2.1 (2023-11-13) 161 | ------------------ 162 | * fix conan 163 | * fix conan 164 | * Contributors: Davide Faconti 165 | 166 | 0.2.0 (2023-11-13) 167 | ------------------ 168 | * First release: supports MCAP sink only 169 | * Contributors: Davide Faconti, Henry Moore 170 | -------------------------------------------------------------------------------- /data_tamer_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | 3 | project(data_tamer_cpp VERSION 1.0.0) 4 | 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 7 | set(CMAKE_CXX_STANDARD 17) 8 | 9 | if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME}) 10 | option(DATA_TAMER_BUILD_TESTS "Build tests" ON) 11 | option(DATA_TAMER_BUILD_EXAMPLES "Build examples" ON) 12 | else() 13 | option(DATA_TAMER_BUILD_TESTS "Build tests" OFF) 14 | option(DATA_TAMER_BUILD_EXAMPLES "Build examples" OFF) 15 | endif() 16 | 17 | option(BUILD_SHARED_LIBS "Build using shared libraries" OFF) 18 | 19 | # optionally build for ROS 2 20 | option(DATA_TAMER_BUILD_ROS "Build for ROS 2" ON) 21 | if (DATA_TAMER_BUILD_ROS) 22 | find_package(ament_cmake QUIET) 23 | if (NOT ament_cmake_FOUND) 24 | set(DATA_TAMER_BUILD_ROS FALSE) 25 | message(WARNING "ament_cmake not found, building without ROS 2 support. Set DATA_TAMER_BUILD_ROS to false to disable this warning.") 26 | endif() 27 | endif() 28 | 29 | ########################################### 30 | # check if mcap can be found. If true, 31 | # probably we used conan. If false, build from 3rdparty 32 | find_package(mcap QUIET) 33 | if(NOT mcap_FOUND AND NOT DATA_TAMER_BUILD_ROS) 34 | set(USE_VENDORED_MCAP ON) 35 | message(STATUS "MCAP from 3rdparty") 36 | add_subdirectory(3rdparty/mcap) 37 | set(mcap_LIBRARY mcap_lib) 38 | else() 39 | message(STATUS "MCAP provided by conan (?)") 40 | set(mcap_LIBRARY mcap::mcap) 41 | endif() 42 | ########################################### 43 | 44 | 45 | if (DATA_TAMER_BUILD_ROS) 46 | set(ROS2_SINK src/sinks/ros2_publisher_sink.cpp) 47 | endif() 48 | 49 | if(BUILD_SHARED_LIBS OR DATA_TAMER_BUILD_ROS) 50 | set(LIB_TYPE SHARED) 51 | else() 52 | set(LIB_TYPE STATIC) 53 | endif() 54 | 55 | if(DATA_TAMER_BUILD_ROS AND NOT BUILD_SHARED_LIBS) 56 | message("BUILD_SHARED_LIBS forced to ON (i.e. SHARED) in ROS2. Set BUILD_SHARED_LIBS to true to disable this warning when building with ROS.") 57 | endif() 58 | 59 | add_library(data_tamer ${LIB_TYPE} 60 | include/data_tamer/channel.hpp 61 | include/data_tamer/custom_types.hpp 62 | include/data_tamer/data_tamer.hpp 63 | include/data_tamer/types.hpp 64 | include/data_tamer/values.hpp 65 | include/data_tamer/sinks/dummy_sink.hpp 66 | include/data_tamer/sinks/mcap_sink.hpp 67 | 68 | src/channel.cpp 69 | src/data_tamer.cpp 70 | src/data_sink.cpp 71 | src/types.cpp 72 | 73 | src/sinks/mcap_sink.cpp 74 | ${ROS2_SINK} 75 | 76 | include/data_tamer/logged_value.hpp 77 | ) 78 | 79 | 80 | if (MSVC) 81 | target_compile_options(data_tamer PRIVATE /W4 /WX) 82 | else() 83 | target_compile_options(data_tamer PRIVATE -Wall -Wconversion -Wextra -Wsign-conversion -Werror -Wpedantic -Wno-sign-conversion) 84 | endif() 85 | 86 | target_compile_features(data_tamer PUBLIC cxx_std_17) 87 | target_include_directories(data_tamer 88 | PUBLIC 89 | $ 90 | $ 91 | PRIVATE 92 | $ 93 | ) 94 | target_compile_definitions(data_tamer PUBLIC -DDATA_TAMER_VERSION="${CMAKE_PROJECT_VERSION}") 95 | 96 | set(INSTALL_TARGETS data_tamer) 97 | 98 | if (DATA_TAMER_BUILD_ROS) 99 | message(STATUS "Compiling for ROS2") 100 | target_compile_definitions(data_tamer PUBLIC USING_ROS2=1 ) 101 | 102 | find_package(mcap_vendor REQUIRED) 103 | find_package(rclcpp REQUIRED) 104 | find_package(rclcpp_lifecycle REQUIRED) 105 | find_package(data_tamer_msgs REQUIRED) 106 | 107 | target_include_directories(data_tamer PUBLIC 108 | ${mcap_vendor_INCLUDE_DIRS} 109 | ${rclcpp_INCLUDE_DIRS} 110 | ${rclcpp_lifecycle_INCLUDE_DIRS} 111 | ${data_tamer_msgs_INCLUDE_DIRS} 112 | ) 113 | 114 | # we need to find the underlying library for `target_link_libraries` to work 115 | find_library(MCAP_LIBRARY mcap PATHS ${mcap_vendor_LIBRARY_DIRS}) 116 | 117 | target_link_libraries(data_tamer PUBLIC 118 | ${mcap_vendor_LIBRARIES} 119 | ${rclcpp_LIBRARIES} 120 | ${rclcpp_lifecycle_LIBRARIES} 121 | ${data_tamer_msgs_LIBRARIES} 122 | ${MCAP_LIBRARY} 123 | ) 124 | 125 | ament_export_targets(data_tamerTargets HAS_LIBRARY_TARGET) 126 | ament_export_dependencies(mcap_vendor rclcpp rclcpp_lifecycle data_tamer_msgs) 127 | ament_package() 128 | 129 | elseif( USE_VENDORED_MCAP ) 130 | target_include_directories(data_tamer PRIVATE 131 | $ ) 132 | target_link_libraries(data_tamer PRIVATE mcap_lib) 133 | list(APPEND INSTALL_TARGETS mcap_lib) 134 | else() 135 | find_package(mcap REQUIRED) 136 | target_link_libraries(data_tamer PRIVATE mcap::mcap) 137 | endif() 138 | 139 | install( 140 | DIRECTORY include/ 141 | DESTINATION include 142 | ) 143 | 144 | 145 | install( 146 | TARGETS ${INSTALL_TARGETS} 147 | EXPORT data_tamerTargets 148 | LIBRARY DESTINATION lib 149 | ARCHIVE DESTINATION lib 150 | RUNTIME DESTINATION bin 151 | INCLUDES DESTINATION include 152 | ) 153 | 154 | if (NOT DATA_TAMER_BUILD_ROS) 155 | 156 | set(LIBRARY_INSTALL_DIR lib) 157 | set(INCLUDE_INSTALL_DIR include) 158 | 159 | # Install config file for finding the package. 160 | include(CMakePackageConfigHelpers) 161 | 162 | configure_package_config_file( 163 | ${CMAKE_CURRENT_SOURCE_DIR}/data_tamerConfig.cmake.in 164 | ${CMAKE_CURRENT_BINARY_DIR}/data_tamerConfig.cmake 165 | INSTALL_DESTINATION lib/cmake/data_tamer 166 | PATH_VARS INCLUDE_INSTALL_DIR) 167 | 168 | write_basic_package_version_file( 169 | "data_tamerConfigVersion.cmake" 170 | VERSION ${PROJECT_VERSION} 171 | COMPATIBILITY SameMajorVersion) 172 | 173 | install(EXPORT data_tamerTargets 174 | FILE data_tamerTargets.cmake 175 | NAMESPACE data_tamer:: 176 | DESTINATION lib/cmake/data_tamer ) 177 | 178 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/data_tamerConfig.cmake" 179 | DESTINATION lib/cmake/data_tamer ) 180 | 181 | endif() 182 | 183 | if(DATA_TAMER_BUILD_TESTS) 184 | include(CTest) 185 | enable_testing() 186 | add_subdirectory(tests) 187 | endif() 188 | 189 | if(DATA_TAMER_BUILD_EXAMPLES) 190 | add_subdirectory(examples) 191 | endif() 192 | 193 | find_package(benchmark QUIET) 194 | if(benchmark_FOUND) 195 | add_subdirectory(benchmarks) 196 | else() 197 | message("Google Benchmark library not found") 198 | endif() 199 | -------------------------------------------------------------------------------- /data_tamer_cpp/benchmarks/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | add_executable(dt_benchmark data_tamer_benchmark.cpp) 3 | target_include_directories(dt_benchmark 4 | PUBLIC $) 5 | target_link_libraries(dt_benchmark data_tamer benchmark) 6 | -------------------------------------------------------------------------------- /data_tamer_cpp/benchmarks/data_tamer_benchmark.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "data_tamer/data_sink.hpp" 3 | #include "data_tamer/data_tamer.hpp" 4 | #include "../examples/geometry_types.hpp" 5 | 6 | using namespace DataTamer; 7 | 8 | class NullSink : public DataSinkBase 9 | { 10 | public: 11 | ~NullSink() override { stopThread(); } 12 | void addChannel(std::string const&, Schema const&) override {} 13 | bool storeSnapshot(const Snapshot&) override { return true; } 14 | }; 15 | 16 | static void DT_Doubles(benchmark::State& state) 17 | { 18 | std::vector values(size_t(state.range(0))); 19 | 20 | auto registry = ChannelsRegistry(); 21 | auto channel = registry.getChannel("channel"); 22 | channel->addDataSink(std::make_shared()); 23 | 24 | channel->registerValue("values", &values); 25 | 26 | for(auto _ : state) 27 | { 28 | channel->takeSnapshot(); 29 | } 30 | } 31 | 32 | static void DT_PoseType(benchmark::State& state) 33 | { 34 | std::vector poses(size_t(state.range(0))); 35 | 36 | auto registry = ChannelsRegistry(); 37 | auto channel = registry.getChannel("channel"); 38 | channel->addDataSink(std::make_shared()); 39 | 40 | channel->registerValue("values", &poses); 41 | 42 | for(auto _ : state) 43 | { 44 | channel->takeSnapshot(); 45 | } 46 | } 47 | 48 | BENCHMARK(DT_Doubles)->Arg(125)->Arg(250)->Arg(500)->Arg(1000)->Arg(2000); 49 | BENCHMARK(DT_PoseType)->Arg(125)->Arg(250)->Arg(500)->Arg(1000); 50 | 51 | BENCHMARK_MAIN(); 52 | -------------------------------------------------------------------------------- /data_tamer_cpp/conanfile.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from conan import ConanFile 3 | from conan.tools.cmake import CMakeToolchain, CMake, cmake_layout, CMakeDeps 4 | 5 | 6 | class DataTamerConan(ConanFile): 7 | name = "data_tamer" 8 | version = "0.9.4" 9 | package_type = "library" 10 | url = "https://github.com/facontidavide/data_tamer" 11 | license = "MIT" 12 | description = "library for data profiling" 13 | settings = "os", "compiler", "build_type", "arch" 14 | options = { 15 | "shared": [True, False], 16 | "fPIC": [True, False], 17 | "tests": [True, False], 18 | "examples": [True, False] 19 | } 20 | default_options = { 21 | "shared": False, 22 | "fPIC": True, 23 | "tests": True, 24 | "examples": True 25 | } 26 | exports_sources = ( 27 | "3rdparty/*", 28 | "include/*", 29 | "src/*", 30 | "examples/*", 31 | "tests/*", 32 | "CMakeLists.txt", 33 | "data_tamerConfig.cmake.in" 34 | ) 35 | 36 | def requirements(self): 37 | 38 | self.requires("mcap/1.3.0") 39 | if self.options.tests: 40 | self.requires("gtest/1.14.0") 41 | 42 | def build_requirements(self): 43 | self.tool_requires("cmake/3.26.4") 44 | 45 | def configure(self): 46 | if self.options.shared: 47 | self.options.rm_safe("fPIC") 48 | 49 | def layout(self): 50 | cmake_layout(self) 51 | 52 | def generate(self): 53 | tc = CMakeToolchain(self) 54 | tc.generate() 55 | 56 | deps = CMakeDeps(self) 57 | deps.generate() 58 | 59 | def build(self): 60 | cmake = CMake(self) 61 | cmake.configure( 62 | { 63 | "DATA_TAMER_BUILD_TESTS": self.options.tests, 64 | "DATA_TAMER_BUILD_EXAMPLES": self.options.examples 65 | } 66 | ) 67 | cmake.build() 68 | 69 | def package(self): 70 | cmake = CMake(self) 71 | cmake.install() 72 | 73 | def package_info(self): 74 | self.cpp_info.libs = ["data_tamer"] 75 | -------------------------------------------------------------------------------- /data_tamer_cpp/data_tamerConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | set(data_tamer_VERSION @VERSION@) 4 | 5 | set_and_check(data_tamer_INCLUDE_DIR "@PACKAGE_INCLUDE_INSTALL_DIR@") 6 | include("${CMAKE_CURRENT_LIST_DIR}/data_tamerTargets.cmake") 7 | set(data_tamer_LIBRARY data_tamer::data_tamer) 8 | -------------------------------------------------------------------------------- /data_tamer_cpp/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | function(CompileExample TARGET) 3 | add_executable(${TARGET} ${TARGET}.cpp) 4 | target_include_directories(${TARGET} 5 | PUBLIC $) 6 | target_link_libraries(${TARGET} data_tamer) 7 | endfunction() 8 | 9 | CompileExample(T01_basic_example) 10 | CompileExample(T02_custom_types) 11 | CompileExample(T03_mcap_writer) 12 | CompileExample(mcap_1m_per_sec) 13 | 14 | add_executable(mcap_reader mcap_reader.cpp) 15 | target_include_directories(mcap_reader 16 | PUBLIC $) 17 | 18 | # optionally build for ROS 2 19 | option(DATA_TAMER_BUILD_ROS "Build for ROS 2" ON) 20 | if ( DATA_TAMER_BUILD_ROS AND ament_cmake_FOUND ) 21 | target_include_directories(mcap_reader PUBLIC 22 | ${mcap_vendor_INCLUDE_DIRS} 23 | ) 24 | target_link_libraries(mcap_reader 25 | data_tamer 26 | ${mcap_vendor_LIBRARIES} 27 | ) 28 | 29 | CompileExample(ros2_publisher) 30 | 31 | install(TARGETS ros2_publisher 32 | DESTINATION lib/${PROJECT_NAME}) 33 | else() 34 | target_link_libraries(mcap_reader data_tamer ${mcap_LIBRARY}) 35 | target_include_directories(mcap_reader 36 | PRIVATE 37 | $ 38 | ) 39 | endif() 40 | -------------------------------------------------------------------------------- /data_tamer_cpp/examples/T01_basic_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "data_tamer/data_tamer.hpp" 3 | #include "data_tamer/sinks/dummy_sink.hpp" 4 | 5 | int main() 6 | { 7 | using namespace DataTamer; 8 | 9 | // start defining one or more Sinks that must be added by default. 10 | // Do addDefaultSink BEFORE creating a channel. 11 | auto dummy_sink = std::make_shared(); 12 | ChannelsRegistry::Global().addDefaultSink(dummy_sink); 13 | 14 | // Create (or get) a channel using the global registry (singleton) 15 | auto channel = ChannelsRegistry::Global().getChannel("chan"); 16 | 17 | // If you don't want to use addDefaultSink, you can do: 18 | // channel->addDataSink(std::make_shared()) 19 | 20 | // You can register any arithmetic value. You are responsible for their lifetime 21 | double value_real = 3.14; 22 | int value_int = 42; 23 | [[maybe_unused]] auto id1 = channel->registerValue("value_real", &value_real); 24 | [[maybe_unused]] auto id2 = channel->registerValue("value_int", &value_int); 25 | 26 | // If you prefer to use RAII, use this method instead 27 | // logged_float will disable itself when it goes out of scope. 28 | auto logged_float = channel->createLoggedValue("real"); 29 | 30 | // this is the way you store the current snapshot of the values 31 | channel->takeSnapshot(); 32 | 33 | // you can modify logged_float like this 34 | logged_float->set(6.28f); 35 | 36 | // if you want to modify it in a thread-safe manner, you can modify it like this 37 | // while ptr exists, its mutex will be locked, so make sure you destruct it as soon as you're done! 38 | if(auto ptr = logged_float->getMutablePtr()) 39 | { 40 | *ptr += 1.1f; 41 | } 42 | 43 | // If you want to access logged_float by reference, but you are not planning to modify its value, 44 | // you should use getConstPtr(), instead. In this way, you will reduce mutex contention. 45 | if(auto ptr = logged_float->getConstPtr()) 46 | { 47 | std::cout << "logged_float = " << *ptr << "\n"; 48 | } 49 | 50 | // You can disable a value like this 51 | channel->setEnabled(id1, false); 52 | // or 53 | logged_float->setEnabled(false); 54 | 55 | // The serialized data of the next snapshot will contain 56 | // only [value_int], i.e. [id2], since the other two are disabled 57 | channel->takeSnapshot(); 58 | } 59 | -------------------------------------------------------------------------------- /data_tamer_cpp/examples/T02_custom_types.cpp: -------------------------------------------------------------------------------- 1 | #include "data_tamer/data_tamer.hpp" 2 | #include "data_tamer/sinks/dummy_sink.hpp" 3 | #include 4 | #include 5 | 6 | // check the custom type in the following file, that defines: 7 | // 8 | // - Point3D 9 | // - Quaternion 10 | // - Pose 11 | 12 | #include "geometry_types.hpp" 13 | 14 | struct Bar 15 | { 16 | int a; 17 | }; 18 | 19 | int main() 20 | { 21 | using namespace TestTypes; 22 | using namespace DataTamer; 23 | 24 | auto dummy_sink = std::make_shared(); 25 | ChannelsRegistry::Global().addDefaultSink(dummy_sink); 26 | auto channel = ChannelsRegistry::Global().getChannel("my_channel"); 27 | 28 | // Register as usual 29 | Point3D point; 30 | Pose pose; 31 | std::vector points_vect(5); 32 | std::array value_array; 33 | 34 | channel->registerValue("point", &point); 35 | channel->registerValue("pose", &pose); 36 | channel->registerValue("points", &points_vect); 37 | channel->registerValue("value_array", &value_array); 38 | 39 | // Print the schema to understand how they are serialized 40 | std::cout << channel->getSchema() << std::endl; 41 | 42 | // Note has the size of the message is almost the same as the raw data. 43 | // The only overhead is the size of points_vect 44 | size_t expected_size = sizeof(double) * 3 + // point 45 | sizeof(double) * 7 + // pose 46 | sizeof(uint32_t) + 47 | 5 * (sizeof(double) * 3) + // points_vect and its size 48 | sizeof(int32_t) * 3; // value_array 49 | 50 | channel->takeSnapshot(); 51 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 52 | 53 | std::cout << "\nMessage size: " << dummy_sink->latest_snapshot.payload.size() 54 | << " expected: " << expected_size << std::endl; 55 | } 56 | -------------------------------------------------------------------------------- /data_tamer_cpp/examples/T03_mcap_writer.cpp: -------------------------------------------------------------------------------- 1 | #include "data_tamer/data_tamer.hpp" 2 | #include "data_tamer/sinks/mcap_sink.hpp" 3 | #include "geometry_types.hpp" 4 | #include 5 | #include 6 | 7 | using namespace DataTamer; 8 | 9 | int main() 10 | { 11 | auto mcap_sink = std::make_shared("test_sample.mcap"); 12 | ChannelsRegistry::Global().addDefaultSink(mcap_sink); 13 | 14 | // Create (or get) a channel using the global registry (singleton) 15 | auto channelA = ChannelsRegistry::Global().getChannel("channelA"); 16 | auto channelB = ChannelsRegistry::Global().getChannel("channelB"); 17 | 18 | // logs in channelA 19 | std::vector v1(10, 0); 20 | std::array v2 = { 1, 2, 3, 4 }; 21 | int32_t v3 = 5; 22 | uint16_t v4 = 6; 23 | TestTypes::Pose pose; 24 | pose.pos = { 1, 2, 3 }; 25 | pose.rot = { 0.4, 0.5, 0.6, 0.7 }; 26 | 27 | channelA->registerValue("vector_10", &v1); 28 | channelA->registerValue("array_4", &v2); 29 | channelA->registerValue("val_int32", &v3); 30 | channelA->registerValue("val_int16", &v4); 31 | channelA->registerValue("pose", &pose); 32 | 33 | // logs in channelB 34 | double v5 = 10; 35 | uint16_t v6 = 11; 36 | std::vector v7(4, 12); 37 | std::array v8 = { 13, 14, 15 }; 38 | std::vector points(2); 39 | points[0] = { 1, 2, 3 }; 40 | points[1] = { 4, 5, 6 }; 41 | 42 | channelB->registerValue("real_value", &v5); 43 | channelB->registerValue("short_int", &v6); 44 | channelB->registerValue("vector_4", &v7); 45 | channelB->registerValue("array_3", &v8); 46 | channelB->registerValue("points", &points); 47 | 48 | for(size_t i = 0; i < 1000; i++) 49 | { 50 | channelA->takeSnapshot(); 51 | if(i % 2 == 0) 52 | { 53 | channelB->takeSnapshot(); 54 | } 55 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 56 | } 57 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 58 | std::cout << "DONE" << std::endl; 59 | } 60 | -------------------------------------------------------------------------------- /data_tamer_cpp/examples/geometry_types.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace TestTypes 5 | { 6 | struct Point3D 7 | { 8 | double x = 0; 9 | double y = 0; 10 | double z = 0; 11 | }; 12 | 13 | struct Quaternion 14 | { 15 | double w = 1; 16 | double x = 0; 17 | double y = 0; 18 | double z = 0; 19 | }; 20 | 21 | struct Pose 22 | { 23 | Point3D pos; 24 | Quaternion rot; 25 | }; 26 | 27 | } // end namespace TestTypes 28 | 29 | namespace PseudoEigen 30 | { 31 | 32 | class Vector2d 33 | { 34 | double _x = 0; 35 | double _y = 0; 36 | 37 | public: 38 | Vector2d() = default; 39 | Vector2d(double x, double y) : _x(x), _y(y) {} 40 | 41 | const double& x() const { return _x; } 42 | const double& y() const { return _y; } 43 | 44 | double& x() { return _x; } 45 | double& y() { return _y; } 46 | }; 47 | 48 | } // namespace PseudoEigen 49 | 50 | namespace TestTypes 51 | { 52 | 53 | template 54 | std::string_view TypeDefinition(Point3D& point, AddField& add) 55 | { 56 | add("x", &point.x); 57 | add("y", &point.y); 58 | add("z", &point.z); 59 | return "Point3D"; 60 | } 61 | 62 | //-------------------------------------------------------------- 63 | // We must specialize the function TypeDefinition 64 | // This must be done in the same namespace of the original type 65 | 66 | template 67 | std::string_view TypeDefinition(Quaternion& quat, AddField& add) 68 | { 69 | add("w", &quat.w); 70 | add("x", &quat.x); 71 | add("y", &quat.y); 72 | add("z", &quat.z); 73 | return "Quaternion"; 74 | } 75 | 76 | template 77 | std::string_view TypeDefinition(Pose& pose, AddField& add) 78 | { 79 | add("position", &pose.pos); 80 | add("rotation", &pose.rot); 81 | return "Pose"; 82 | } 83 | 84 | } // end namespace TestTypes 85 | 86 | namespace PseudoEigen 87 | { 88 | template 89 | std::string_view TypeDefinition(Vector2d& vect, AddField& add) 90 | { 91 | add("x", &vect.x()); 92 | add("y", &vect.y()); 93 | return "Vector2d"; 94 | } 95 | 96 | } // end namespace PseudoEigen 97 | -------------------------------------------------------------------------------- /data_tamer_cpp/examples/mcap_1m_per_sec.cpp: -------------------------------------------------------------------------------- 1 | #include "data_tamer/data_tamer.hpp" 2 | #include "data_tamer/sinks/mcap_sink.hpp" 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace DataTamer; 8 | 9 | void WritingThread(const std::string& channel_name) 10 | { 11 | std::cout << "Starting thread: " << channel_name << std::endl; 12 | 13 | // Create (or get) a channel using the global registry (singleton) 14 | auto channel = ChannelsRegistry::Global().getChannel(channel_name); 15 | long time_diff_nsec = 0; 16 | 17 | auto const vect_size = 100; 18 | std::vector real64(vect_size); 19 | std::vector real32(vect_size); 20 | std::vector int16(vect_size); 21 | 22 | enum Color : uint8_t 23 | { 24 | RED = 0, 25 | GREEN = 1, 26 | BLUE = 2 27 | }; 28 | Color color; 29 | 30 | channel->registerValue("real64", &real64); 31 | channel->registerValue("real32", &real32); 32 | channel->registerValue("int16", &int16); 33 | channel->registerValue("color", &color); 34 | 35 | int count = 0; 36 | double t = 0; 37 | while(t < 10) // 10 simulated seconds 38 | { 39 | auto S = std::sin(t); 40 | for(size_t i = 0; i < vect_size; i++) 41 | { 42 | const auto value = static_cast(i) + S; 43 | real64[i] = value; 44 | real32[i] = float(value); 45 | int16[i] = int16_t(10 * value); 46 | } 47 | color = static_cast(count % 3); 48 | 49 | if(count++ % 1000 == 0) 50 | { 51 | printf("[%s] time: %.1f\n", channel_name.c_str(), t); 52 | std::flush(std::cout); 53 | } 54 | auto t1 = std::chrono::system_clock::now(); 55 | if(!channel->takeSnapshot()) 56 | { 57 | std::cout << "pushing failed\n"; 58 | } 59 | auto t2 = std::chrono::system_clock::now(); 60 | 61 | auto diff = std::chrono::duration_cast(t2 - t1); 62 | time_diff_nsec += diff.count(); 63 | 64 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 65 | t += 0.001; 66 | } 67 | std::cout << "average execution time of takeSnapshot(): " << time_diff_nsec / count 68 | << " nanoseconds" << std::endl; 69 | } 70 | 71 | int main() 72 | { 73 | // Start defining one or more Sinks that must be added by default. 74 | // Do this BEFORE creating a channel. 75 | auto mcap_sink = std::make_shared("test_1M.mcap"); 76 | ChannelsRegistry::Global().addDefaultSink(mcap_sink); 77 | 78 | // Create (or get) a channel using the global registry (singleton) 79 | auto channel = ChannelsRegistry::Global().getChannel("chan"); 80 | 81 | // Each WritingThread has 300 traced values. 82 | // In total, we will collect 300*4 = 1200 traced values at 1 KHz 83 | // for 10 seconds (12 million data points) 84 | const int N = 4; 85 | std::thread writers[N]; 86 | for(int i = 0; i < N; i++) 87 | { 88 | writers[i] = std::thread(WritingThread, std::string("channel_") + std::to_string(i)); 89 | } 90 | 91 | for(int i = 0; i < N; i++) 92 | { 93 | writers[i].join(); 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /data_tamer_cpp/examples/mcap_reader.cpp: -------------------------------------------------------------------------------- 1 | #include "data_tamer_parser/data_tamer_parser.hpp" 2 | #include 3 | 4 | // Try reading the generated [test_sample.mcap] 5 | // using examples/mcap_writer_sample.cpp 6 | int main(int argc, char** argv) 7 | { 8 | if(argc != 2) 9 | { 10 | std::cout << "add the MCAP file as argument" << std::endl; 11 | return 1; 12 | } 13 | std::string filepath = argv[1]; 14 | 15 | // open the file 16 | mcap::McapReader reader; 17 | { 18 | auto const res = reader.open(filepath); 19 | if(!res.ok()) 20 | { 21 | throw std::runtime_error("Can't open MCAP file"); 22 | } 23 | } 24 | 25 | // start reading all the schemas and parsing them 26 | std::unordered_map schema_id_to_hash; 27 | std::unordered_map hash_to_schema; 28 | // must call this, before accessing the schemas 29 | auto summary = reader.readSummary(mcap::ReadSummaryMethod::NoFallbackScan); 30 | for(const auto& [schema_id, mcap_schema] : reader.schemas()) 31 | { 32 | std::string schema_text(reinterpret_cast(mcap_schema->data.data()), 33 | mcap_schema->data.size()); 34 | 35 | auto dt_schema = DataTamerParser::BuilSchemaFromText(schema_text); 36 | schema_id_to_hash[mcap_schema->id] = dt_schema.hash; 37 | hash_to_schema[dt_schema.hash] = dt_schema; 38 | } 39 | 40 | // this application will do nothing with the actual data. We will simple count the 41 | // number of messages per time series 42 | using MessageCount = std::map; 43 | std::map message_counts_per_channel; 44 | 45 | auto IncrementCounter = [&](const std::string& series_name, 46 | MessageCount& message_counts) { 47 | auto it = message_counts.find(series_name); 48 | if(it == message_counts.end()) 49 | { 50 | message_counts[series_name] = 1; 51 | } 52 | else 53 | { 54 | it->second++; 55 | } 56 | }; 57 | 58 | // parse all messages 59 | for(const auto& msg : reader.readMessages()) 60 | { 61 | // start updating the fields of SnapshotView 62 | DataTamerParser::SnapshotView snapshot; 63 | snapshot.timestamp = msg.message.logTime; 64 | snapshot.schema_hash = schema_id_to_hash.at(msg.schema->id); 65 | const auto& dt_schema = hash_to_schema.at(snapshot.schema_hash); 66 | 67 | // msg_buffer contains both active_mask and payload, serialized 68 | // one after the other. 69 | DataTamerParser::BufferSpan msg_buffer = { 70 | reinterpret_cast(msg.message.data), msg.message.dataSize 71 | }; 72 | 73 | const uint32_t mask_size = DataTamerParser::Deserialize(msg_buffer); 74 | snapshot.active_mask.data = msg_buffer.data; 75 | snapshot.active_mask.size = mask_size; 76 | msg_buffer.trimFront(mask_size); 77 | 78 | const uint32_t payload_size = DataTamerParser::Deserialize(msg_buffer); 79 | snapshot.payload.data = msg_buffer.data; 80 | snapshot.payload.size = payload_size; 81 | 82 | // prepare the callback to be invoked by ParseSnapshot. 83 | // Wrap IncrementCounter to add the channel_name 84 | const std::string& channel_name = msg.channel->topic; 85 | auto& message_counts = message_counts_per_channel[channel_name]; 86 | auto callback_number = [&](const std::string& series_name, 87 | const DataTamerParser::VarNumber&) { 88 | IncrementCounter(series_name, message_counts); 89 | }; 90 | 91 | DataTamerParser::ParseSnapshot(dt_schema, snapshot, callback_number); 92 | } 93 | 94 | // display the counted data samples 95 | for(const auto& [channel_name, msg_counts] : message_counts_per_channel) 96 | { 97 | std::cout << channel_name << ":" << std::endl; 98 | for(const auto& [name, count] : msg_counts) 99 | { 100 | std::cout << " " << name << ":" << count << std::endl; 101 | } 102 | } 103 | return 0; 104 | } 105 | -------------------------------------------------------------------------------- /data_tamer_cpp/examples/ros2_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "data_tamer/data_tamer.hpp" 2 | #include "data_tamer/sinks/ros2_publisher_sink.hpp" 3 | 4 | #include 5 | 6 | using namespace DataTamer; 7 | 8 | int main(int argc, char* argv[]) 9 | { 10 | rclcpp::init(argc, argv); 11 | auto node = std::make_shared("test_datatamer"); 12 | 13 | auto ros2_sink = std::make_shared(node, "test"); 14 | ChannelsRegistry::Global().addDefaultSink(ros2_sink); 15 | 16 | // Create (or get) a channel using the global registry (singleton) 17 | auto channel = ChannelsRegistry::Global().getChannel("channel"); 18 | 19 | auto const vect_size = 100; 20 | std::vector real64(vect_size); 21 | std::vector real32(vect_size); 22 | std::vector int16(vect_size); 23 | 24 | channel->registerValue("real64", &real64); 25 | channel->registerValue("real32", &real32); 26 | channel->registerValue("int16", &int16); 27 | 28 | int count = 0; 29 | double t = 0; 30 | 31 | while(rclcpp::ok()) 32 | { 33 | double S = std::sin(t); 34 | for(size_t i = 0; i < vect_size; i++) 35 | { 36 | const double val = double(i) + S; 37 | real64[i] = val; 38 | real32[i] = float(val); 39 | int16[i] = int16_t(10 * (val)); 40 | } 41 | if(count++ % 500 == 0) 42 | { 43 | RCLCPP_INFO(node->get_logger(), "snapshots: %d\n", count); 44 | } 45 | if(!channel->takeSnapshot()) 46 | { 47 | RCLCPP_ERROR(node->get_logger(), "pushing failed"); 48 | } 49 | std::this_thread::sleep_for(std::chrono::milliseconds(5)); 50 | t += 0.005; 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /data_tamer_cpp/include/data_tamer/custom_types.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "data_tamer/types.hpp" 7 | #include "data_tamer/contrib/SerializeMe.hpp" 8 | 9 | namespace DataTamer 10 | { 11 | 12 | class CustomSerializer 13 | { 14 | public: 15 | using Ptr = std::shared_ptr; 16 | 17 | virtual ~CustomSerializer() = default; 18 | // name of the type, to be written in the schema string. 19 | virtual const std::string& typeName() const = 0; 20 | 21 | // optional custom schema of the type 22 | virtual std::optional typeSchema() const { return std::nullopt; } 23 | // size in bytes of the serialized object. 24 | // Needed to pre-allocate memory in the buffer 25 | virtual size_t serializedSize(const void* instance) const = 0; 26 | 27 | // true if the method serializedSize will ALWAYS return the same value 28 | virtual bool isFixedSize() const = 0; 29 | 30 | // serialize an object into a buffer. 31 | virtual void serialize(const void* instance, SerializeMe::SpanBytes&) const = 0; 32 | }; 33 | 34 | //------------------------------------------------------------------ 35 | 36 | // This derived class is used automatically by all the types 37 | // that have a template specialization of TypeDefinition 38 | template 39 | class CustomSerializerT : public CustomSerializer 40 | { 41 | public: 42 | CustomSerializerT(std::string type_name); 43 | 44 | const std::string& typeName() const override; 45 | 46 | size_t serializedSize(const void* src_instance) const override; 47 | 48 | bool isFixedSize() const override; 49 | 50 | void serialize(const void* src_instance, 51 | SerializeMe::SpanBytes& dst_buffer) const override; 52 | 53 | private: 54 | std::string _name; 55 | size_t _fixed_size = 0; 56 | }; 57 | 58 | class TypesRegistry 59 | { 60 | public: 61 | template 62 | CustomSerializer::Ptr addType(const std::string& type_name, 63 | bool skip_if_present = false); 64 | 65 | template 66 | [[nodiscard]] CustomSerializer::Ptr getSerializer(); 67 | 68 | private: 69 | std::unordered_map _types; 70 | std::recursive_mutex _mutex; 71 | }; 72 | 73 | //------------------------------------------------------------------ 74 | //------------------------------------------------------------------ 75 | //------------------------------------------------------------------ 76 | 77 | template 78 | struct CustomTypeName 79 | { 80 | static constexpr std::string_view get() 81 | { 82 | static_assert(std::is_default_constructible_v, "Must be default constructible"); 83 | static_assert(SerializeMe::has_TypeDefinition(), "Missing TypeDefinition"); 84 | T dummy; 85 | return TypeDefinition(dummy, SerializeMe::EmptyFuncion); 86 | } 87 | }; 88 | 89 | template