├── .gitignore ├── conanfile.txt ├── CONTRIBUTORS.md ├── .github └── workflows │ └── ros2.yaml ├── .clang-format ├── include └── rosx_introspection │ ├── ros_utils │ ├── ros1_helpers.hpp │ └── ros2_helpers.hpp │ ├── serializer.hpp │ ├── details │ ├── exceptions.hpp │ └── conversion_impl.hpp │ ├── ros_message.hpp │ ├── ros_field.hpp │ ├── deserializer.hpp │ ├── stringtree_leaf.hpp │ ├── ros_type.hpp │ ├── builtin_types.hpp │ ├── tree.hpp │ ├── ros_parser.hpp │ ├── contrib │ ├── msgpack.hpp │ └── nanocdr.hpp │ └── variant.hpp ├── .pre-commit-config.yaml ├── cmake ├── get_cpm.cmake └── CPM.cmake ├── package.xml ├── src ├── ros_utils │ ├── ros2_helpers.cpp │ ├── message_definition_cache.hpp │ └── message_definition_cache.cpp ├── serializer.cpp ├── ros_type.cpp ├── stringtree_leaf.cpp ├── ros_field.cpp ├── deserializer.cpp └── ros_message.cpp ├── README.md ├── python ├── CMakeLists.txt ├── rosx_introspection_py.cpp └── mcap_ros_parser.py ├── CHANGELOG.rst ├── test ├── test_ros1.cpp ├── test_encoding.cpp ├── test_parser.cpp └── test_ros2.cpp ├── CMakeLists.txt └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | /build/* 2 | *.csv 3 | /build* 4 | -------------------------------------------------------------------------------- /conanfile.txt: -------------------------------------------------------------------------------- 1 | [requires] 2 | fast-cdr/2.2.0 3 | rapidjson/cci.20230929 4 | 5 | [generators] 6 | CMakeDeps 7 | CMakeToolchain 8 | -------------------------------------------------------------------------------- /CONTRIBUTORS.md: -------------------------------------------------------------------------------- 1 | # Contributors 2 | 3 | ## Special thanks for all the people who had helped this project so far: 4 | 5 | * [ahmad_ra](https://github.com/ahmad-ra) 6 | -------------------------------------------------------------------------------- /.github/workflows/ros2.yaml: -------------------------------------------------------------------------------- 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: jazzy, ROS_REPO: main} 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v3 15 | - uses: 'ros-industrial/industrial_ci@master' 16 | env: ${{matrix.env}} 17 | with: 18 | package-name: rosx_introspection 19 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | # We base our styles on the standard Google C++ guide with some tweaks 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | 5 | UseTab: Never 6 | 7 | # Tab width is defined as four spaces, as is the indent width. This means each 8 | # indentation level will be a single tab. 9 | TabWidth: 2 10 | IndentWidth: 2 11 | 12 | ColumnLimit: 120 13 | 14 | BreakBeforeBraces: Attach 15 | 16 | InsertBraces: true 17 | 18 | AlignAfterOpenBracket: AlwaysBreak 19 | 20 | 21 | AllowShortBlocksOnASingleLine: Empty 22 | AllowShortFunctionsOnASingleLine: Empty 23 | AllowShortIfStatementsOnASingleLine: Never 24 | -------------------------------------------------------------------------------- /include/rosx_introspection/ros_utils/ros1_helpers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "rosx_introspection/ros_field.hpp" 6 | 7 | namespace RosMsgParser { 8 | 9 | template 10 | inline std::string GetMessageDefinition() { 11 | return ros::message_traits::Definition::value(); 12 | } 13 | 14 | template 15 | inline ROSType GetMessageType() { 16 | return ROSType(ros::message_traits::DataType::value()); 17 | } 18 | 19 | template 20 | inline std::vector BuildMessageBuffer(const T& msg) { 21 | std::vector buffer(ros::serialization::serializationLength(msg)); 22 | ros::serialization::OStream stream(buffer.data(), buffer.size()); 23 | ros::serialization::Serializer::write(stream, msg); 24 | return buffer; 25 | } 26 | 27 | } // namespace RosMsgParser 28 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | exclude: ^.vscode/|^deps/|contrib 2 | repos: 3 | 4 | # Standard hooks 5 | - repo: https://github.com/pre-commit/pre-commit-hooks 6 | rev: v4.5.0 7 | hooks: 8 | - id: check-added-large-files 9 | - id: check-ast 10 | - id: check-case-conflict 11 | - id: check-merge-conflict 12 | - id: check-symlinks 13 | - id: check-xml 14 | - id: check-yaml 15 | exclude: .gitlab-ci.yml 16 | - id: debug-statements 17 | - id: end-of-file-fixer 18 | exclude_types: [svg] 19 | - id: mixed-line-ending 20 | - id: trailing-whitespace 21 | exclude_types: [svg] 22 | - id: fix-byte-order-marker 23 | 24 | # CPP hooks 25 | - repo: https://github.com/pre-commit/mirrors-clang-format 26 | rev: v17.0.6 27 | hooks: 28 | - id: clang-format 29 | types_or: [c++, c] 30 | args: ['-fallback-style=none', '-i'] 31 | -------------------------------------------------------------------------------- /cmake/get_cpm.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: MIT 2 | # 3 | # SPDX-FileCopyrightText: Copyright (c) 2019-2023 Lars Melchior and contributors 4 | 5 | set(CPM_DOWNLOAD_VERSION 1.0.0-development-version) 6 | set(CPM_HASH_SUM "CPM_HASH_SUM_PLACEHOLDER") 7 | 8 | if(CPM_SOURCE_CACHE) 9 | set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 10 | elseif(DEFINED ENV{CPM_SOURCE_CACHE}) 11 | set(CPM_DOWNLOAD_LOCATION "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 12 | else() 13 | set(CPM_DOWNLOAD_LOCATION "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 14 | endif() 15 | 16 | # Expand relative path. This is important if the provided path contains a tilde (~) 17 | get_filename_component(CPM_DOWNLOAD_LOCATION ${CPM_DOWNLOAD_LOCATION} ABSOLUTE) 18 | 19 | file(DOWNLOAD 20 | https://github.com/cpm-cmake/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake 21 | ${CPM_DOWNLOAD_LOCATION} EXPECTED_HASH SHA256=${CPM_HASH_SUM} 22 | ) 23 | 24 | include(${CPM_DOWNLOAD_LOCATION}) 25 | -------------------------------------------------------------------------------- /cmake/CPM.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: MIT 2 | # 3 | # SPDX-FileCopyrightText: Copyright (c) 2019-2023 Lars Melchior and contributors 4 | 5 | set(CPM_DOWNLOAD_VERSION 0.40.0) 6 | set(CPM_HASH_SUM "7b354f3a5976c4626c876850c93944e52c83ec59a159ae5de5be7983f0e17a2a") 7 | 8 | if(CPM_SOURCE_CACHE) 9 | set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 10 | elseif(DEFINED ENV{CPM_SOURCE_CACHE}) 11 | set(CPM_DOWNLOAD_LOCATION "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 12 | else() 13 | set(CPM_DOWNLOAD_LOCATION "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 14 | endif() 15 | 16 | # Expand relative path. This is important if the provided path contains a tilde (~) 17 | get_filename_component(CPM_DOWNLOAD_LOCATION ${CPM_DOWNLOAD_LOCATION} ABSOLUTE) 18 | 19 | file(DOWNLOAD 20 | https://github.com/cpm-cmake/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake 21 | ${CPM_DOWNLOAD_LOCATION} EXPECTED_HASH SHA256=${CPM_HASH_SUM} 22 | ) 23 | 24 | include(${CPM_DOWNLOAD_LOCATION}) 25 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosx_introspection 4 | 2.0.1 5 | Parse any ROS/ROS2 message without compile-time information 6 | 7 | Davide Faconti 8 | MIT 9 | 10 | Davide Faconti 11 | 12 | catkin 13 | ament_cmake 14 | 15 | roscpp 16 | roscpp_serialization 17 | 18 | ament_index_cpp 19 | rclcpp 20 | rosbag2_cpp 21 | 22 | rapidjson-dev 23 | 24 | sensor_msgs 25 | geometry_msgs 26 | 27 | 28 | catkin 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/ros_utils/ros2_helpers.cpp: -------------------------------------------------------------------------------- 1 | #include "rosx_introspection/ros_utils/ros2_helpers.hpp" 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "message_definition_cache.hpp" 8 | 9 | namespace RosMsgParser { 10 | 11 | RmwInterface::RmwSerializedPtr RmwInterface::get_initialized_serialized_message(size_t capacity) { 12 | auto msg = new rmw_serialized_message_t; 13 | *msg = rmw_get_zero_initialized_serialized_message(); 14 | auto ret = rmw_serialized_message_init(msg, capacity, &rcutils_allocator_); 15 | if (ret != RCUTILS_RET_OK) { 16 | throw std::runtime_error( 17 | "Error allocating resources for serialized message: " + std::string(rcutils_get_error_string().str)); 18 | } 19 | 20 | auto serialized_message = std::shared_ptr(msg, [](rmw_serialized_message_t* msg) { 21 | int error = rmw_serialized_message_fini(msg); 22 | delete msg; 23 | if (error != RCUTILS_RET_OK) { 24 | RCUTILS_LOG_ERROR_NAMED("rosbag2_test_common", "Leaking memory. Error: %s", rcutils_get_error_string().str); 25 | } 26 | }); 27 | return serialized_message; 28 | } 29 | 30 | RmwInterface::RmwInterface() { 31 | rcutils_allocator_ = rcutils_get_default_allocator(); 32 | } 33 | 34 | std::string GetMessageDefinition(const std::string& datatype) { 35 | RosMsgParser::MessageDefinitionCache cache; 36 | return cache.get_full_text(datatype); 37 | } 38 | 39 | } // namespace RosMsgParser 40 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS X Introspection 2 | 3 | Unified successor of the following libraries: 4 | 5 | - [ros_type_introspection](https://github.com/facontidavide/ros_type_introspection) 6 | - [ros_msg_parser](https://github.com/facontidavide/ros_msg_parser) 7 | - [ros2_introspection](https://github.com/facontidavide/ros2_introspection) 8 | 9 | The library compiles either using: 10 | - ROS1 (catkin), 11 | - ROS2 (colcon/ament) or 12 | - without any ROS dependency (vanilla cmake). 13 | 14 | To parse any ROS message at runtime, it requires: 15 | 16 | - The name of the type (for instance "sensors_msgs/JointState") 17 | - The definition of the type 18 | (for instance [this one](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/JointState.html)), 19 | - The raw memory buffer to be deserialized into individual key/values pairs. 20 | 21 | The raw memory buffer is usually obtained by: 22 | 23 | - [rosbag::MessageInstance](https://docs.ros.org/en/noetic/api/rosbag_storage/html/c++/classrosbag_1_1MessageInstance.html) and 24 | [Topic::ShapeShifter](http://docs.ros.org/en/noetic/api/topic_tools/html/shape__shifter_8h.html) 25 | in **ROS1**. 26 | - [GenericSubscriber](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/src/rclcpp/generic_subscription.cpp) 27 | or `rosbag2_storage::SerializedBagMessage` on **ROS2**. 28 | - [MCAP](https://github.com/foxglove/mcap). 29 | 30 | 31 | # Python binding 32 | 33 | Compilation instructions: 34 | 35 | ``` 36 | cmake -S. -B build_python -DCMAKE_BUILD_TYPE=Release -DROSX_PYTHON_BINDINGS=ON 37 | cmake --build build_python 38 | ``` 39 | 40 | If you have a rosbag (MCAP file) you can test it with this command 41 | 42 | ``` 43 | PYTHONPATH=build_python/python python3 python/mcap_ros_parser.py path_to_your_rosbag.mcap 44 | ``` 45 | 46 | Note that we are providing the folder where the dynamic library is using **PYTHONPATH**. 47 | -------------------------------------------------------------------------------- /python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15) 2 | project(rosx_introspection_python) 3 | 4 | # Find Python 5 | find_package(Python 3.8 6 | REQUIRED COMPONENTS Interpreter Development.Module 7 | OPTIONAL_COMPONENTS Development.SABIModule) 8 | 9 | # Fetch nanobind via CPM 10 | CPMAddPackage( 11 | NAME nanobind 12 | GITHUB_REPOSITORY wjakob/nanobind 13 | GIT_TAG v2.9.2 14 | ) 15 | 16 | # Create the Python module 17 | nanobind_add_module( 18 | rosx_introspection_py 19 | STABLE_ABI 20 | NB_STATIC # Build static for better portability 21 | rosx_introspection_py.cpp 22 | ) 23 | 24 | # Link with the main rosx_introspection library 25 | target_link_libraries(rosx_introspection_py PRIVATE rosx_introspection) 26 | 27 | # Set properties 28 | set_target_properties(rosx_introspection_py PROPERTIES 29 | CXX_STANDARD 17 30 | CXX_STANDARD_REQUIRED ON 31 | OUTPUT_NAME "rosx_introspection" # This makes the .so file have the right name 32 | ) 33 | 34 | # Determine installation directory 35 | # When building with scikit-build-core, use relative paths 36 | # Otherwise, use the system Python site-packages 37 | if(DEFINED SKBUILD) 38 | set(ROSX_INSTALL_DIR "rosx_introspection") 39 | else() 40 | set(ROSX_INSTALL_DIR "${Python_SITEARCH}/rosx_introspection") 41 | endif() 42 | 43 | # Install the Python module 44 | install(TARGETS rosx_introspection_py 45 | LIBRARY DESTINATION ${ROSX_INSTALL_DIR}) 46 | 47 | # Create __init__.py 48 | file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/__init__.py" 49 | "\"\"\"ROS X Introspection - Python bindings for ROS message parsing.\"\"\" 50 | 51 | from .rosx_introspection import Parser 52 | 53 | __version__ = '1.0.0' 54 | __all__ = ['Parser', 'parse_ros_message'] 55 | ") 56 | 57 | # Install __init__.py 58 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/__init__.py" 59 | DESTINATION ${ROSX_INSTALL_DIR}) 60 | -------------------------------------------------------------------------------- /src/ros_utils/message_definition_cache.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022, Foxglove Technologies. All rights reserved. 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 | #ifndef MESSAGE_DEFINITION_CACHE_HPP_ 16 | #define MESSAGE_DEFINITION_CACHE_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace RosMsgParser { 24 | 25 | struct MessageSpec { 26 | MessageSpec(std::string text, const std::string& package_context); 27 | const std::set dependencies; 28 | const std::string text; 29 | }; 30 | 31 | class MessageDefinitionCache final { 32 | public: 33 | /** 34 | * Concatenate the message definition with its dependencies into a self-contained 35 | * schema. Uses a format similar to ROS 1's gendeps: 36 | * https://github.com/ros/ros/blob/93d8da32091b8b43702eab5d3202f4511dfeb7dc/core/roslib/src/roslib/gentools.py#L239 37 | */ 38 | std::string get_full_text(const std::string& datatype); 39 | 40 | private: 41 | /** 42 | * Load and parse the message file referenced by the given datatype, or return it from 43 | * msg_specs_by_datatype 44 | */ 45 | const MessageSpec& load_message_spec(const std::string& datatype); 46 | 47 | std::unordered_map msg_specs_by_datatype_; 48 | }; 49 | 50 | } // namespace RosMsgParser 51 | 52 | #endif // MESSAGE_DEFINITION_CACHE_HPP_ 53 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosx_introspection 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.1 (2025-10-01) 6 | ------------------ 7 | * Merge pull request `#21 `_ from bryzhao/fix/scikit-build-core-compatibility 8 | Fix CMakeLists to support scikit-build-core installations 9 | * Fix CMakeLists to support scikit-build-core installations 10 | * Merge pull request `#20 `_ from nealtanner/bugfix/mac-compliation 11 | fix compilation errors on mac 12 | * replace sprintf with snprintf 13 | * inline vs constexpr 14 | * Contributors: Bryan Zhao, Davide Faconti, Neal Tanner 15 | 16 | 2.0.0 (2025-09-24) 17 | ------------------ 18 | * optionally remove JSON support 19 | * new formatting 20 | * fix memory and add README 21 | * add python binding 22 | * NanoCDR 23 | * Merge pull request `#16 `_ from traversaro/patch-1 24 | Add missing include of functional header in message_definition_cache.cpp 25 | * Fix issue `#13 `_ 26 | * Merge pull request `#11 `_ from valgur/bugfix/clang 27 | Fix ros_parser.cpp compilation failures on Clang 28 | * Contributors: Davide Faconti, Martin Valgur, Silvio Traversaro 29 | 30 | 1.0.2 (2024-07-28) 31 | ------------------ 32 | * bug fix 33 | * add unit tests 34 | * Contributors: Davide Faconti 35 | 36 | 1.0.1 (2024-06-29) 37 | ------------------ 38 | * use CPM to download dependenies 39 | * Merge pull request `#9 `_ from valgur/feature/cmake-improvements 40 | * Contributors: Davide Faconti, Martin Valgur 41 | 42 | 1.0.0 (2024-06-26) 43 | ------------------ 44 | * New version including JSON conversion 45 | * Contributors: Basavaraj-PN, Davide Faconti, ahmad-ra 46 | -------------------------------------------------------------------------------- /src/serializer.cpp: -------------------------------------------------------------------------------- 1 | #include "rosx_introspection/serializer.hpp" 2 | 3 | #include "rosx_introspection/contrib/nanocdr.hpp" 4 | namespace RosMsgParser { 5 | 6 | NanoCDR_Serializer::NanoCDR_Serializer() { 7 | _storage.reserve(1024); 8 | _cdr_encoder = std::make_shared(nanocdr::CdrHeader(), _storage); 9 | // Write the CDR header to the first 4 bytes 10 | } 11 | 12 | void NanoCDR_Serializer::serialize(BuiltinType type, const Variant& val) { 13 | switch (type) { 14 | case BuiltinType::CHAR: 15 | case BuiltinType::UINT8: 16 | _cdr_encoder->encode(val.convert()); 17 | break; 18 | 19 | case BuiltinType::BOOL: 20 | case BuiltinType::BYTE: 21 | case BuiltinType::INT8: 22 | _cdr_encoder->encode(val.convert()); 23 | break; 24 | 25 | case BuiltinType::UINT16: 26 | _cdr_encoder->encode(val.convert()); 27 | break; 28 | case BuiltinType::INT16: 29 | _cdr_encoder->encode(val.convert()); 30 | break; 31 | case BuiltinType::UINT32: 32 | _cdr_encoder->encode(val.convert()); 33 | break; 34 | case BuiltinType::INT32: 35 | _cdr_encoder->encode(val.convert()); 36 | break; 37 | case BuiltinType::UINT64: 38 | _cdr_encoder->encode(val.convert()); 39 | break; 40 | case BuiltinType::INT64: 41 | _cdr_encoder->encode(val.convert()); 42 | break; 43 | case BuiltinType::FLOAT32: 44 | _cdr_encoder->encode(val.convert()); 45 | break; 46 | case BuiltinType::FLOAT64: 47 | _cdr_encoder->encode(val.convert()); 48 | break; 49 | default: 50 | throw std::runtime_error("Unsupported type"); 51 | } 52 | } 53 | 54 | void NanoCDR_Serializer::serializeString(const std::string& str) { 55 | _cdr_encoder->encode(str); 56 | } 57 | 58 | void NanoCDR_Serializer::serializeUInt32(uint32_t value) { 59 | _cdr_encoder->encode(value); 60 | } 61 | 62 | void NanoCDR_Serializer::reset() { 63 | _cdr_encoder = std::make_shared(nanocdr::CdrHeader(), _storage); 64 | } 65 | 66 | const char* NanoCDR_Serializer::getBufferData() const { 67 | return reinterpret_cast(_storage.data()); 68 | } 69 | 70 | size_t NanoCDR_Serializer::getBufferSize() const { 71 | return _storage.size(); 72 | } 73 | 74 | } // namespace RosMsgParser 75 | -------------------------------------------------------------------------------- /include/rosx_introspection/serializer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // API adapted to FastCDR 4 | 5 | #include 6 | #include 7 | 8 | #include "rosx_introspection/builtin_types.hpp" 9 | #include "rosx_introspection/variant.hpp" 10 | 11 | namespace nanocdr { 12 | class Encoder; 13 | } // namespace nanocdr 14 | 15 | namespace RosMsgParser { 16 | 17 | class Serializer { 18 | public: 19 | virtual ~Serializer() = default; 20 | 21 | virtual bool isROS2() const = 0; 22 | 23 | virtual void serialize(BuiltinType type, const Variant& val) = 0; 24 | 25 | virtual void serializeString(const std::string& str) = 0; 26 | 27 | virtual void serializeUInt32(uint32_t value) = 0; 28 | 29 | virtual void reset() = 0; 30 | 31 | virtual const char* getBufferData() const = 0; 32 | 33 | virtual size_t getBufferSize() const = 0; 34 | }; 35 | 36 | //----------------------------------------------------------------- 37 | 38 | // Specialization of serializer that works with ROS1 39 | class ROS_Serializer : public Serializer { 40 | public: 41 | bool isROS2() const override { 42 | return false; 43 | } 44 | 45 | void serialize(BuiltinType type, const Variant& val) override; 46 | 47 | void serializeString(const std::string& str) override; 48 | 49 | void serializeUInt32(uint32_t value) override; 50 | 51 | void reset() override; 52 | 53 | const char* getBufferData() const override; 54 | 55 | size_t getBufferSize() const override; 56 | 57 | protected: 58 | std::vector _buffer; 59 | size_t _current_size = 0; 60 | 61 | template 62 | T serialize(const T& val) { 63 | T out; 64 | if (_current_size + sizeof(T) > _buffer.size()) { 65 | _buffer.resize((_current_size * 3) / 2); 66 | } 67 | auto* ptr = &(_buffer[_current_size]); 68 | *(reinterpret_cast(ptr)) = val; 69 | _current_size += sizeof(T); 70 | return out; 71 | } 72 | }; 73 | 74 | //----------------------------------------------------------------- 75 | 76 | // Specialization od deserializer that works with ROS2 77 | // wrapping FastCDR 78 | class NanoCDR_Serializer : public Serializer { 79 | public: 80 | NanoCDR_Serializer(); 81 | 82 | bool isROS2() const override { 83 | return true; 84 | } 85 | 86 | void serialize(BuiltinType type, const Variant& val) override; 87 | 88 | void serializeString(const std::string& str) override; 89 | 90 | void serializeUInt32(uint32_t value) override; 91 | 92 | void reset() override; 93 | 94 | const char* getBufferData() const override; 95 | 96 | size_t getBufferSize() const override; 97 | 98 | protected: 99 | std::vector _storage; 100 | std::shared_ptr _cdr_encoder; 101 | }; 102 | 103 | using ROS2_Serializer = NanoCDR_Serializer; 104 | 105 | } // namespace RosMsgParser 106 | -------------------------------------------------------------------------------- /include/rosx_introspection/details/exceptions.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2016-2017 Davide Faconti 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * *******************************************************************/ 34 | 35 | #ifndef VARIANT_NUMBER_EXCEPTIONS_H 36 | #define VARIANT_NUMBER_EXCEPTIONS_H 37 | 38 | #include 39 | #include 40 | 41 | namespace RosMsgParser { 42 | 43 | class RangeException : public std::exception { 44 | public: 45 | explicit RangeException(const char* message) : msg_(message) {} 46 | explicit RangeException(const std::string& message) : msg_(message) {} 47 | ~RangeException() throw() {} 48 | const char* what() const throw() { 49 | return msg_.c_str(); 50 | } 51 | 52 | protected: 53 | std::string msg_; 54 | }; 55 | 56 | class TypeException : public std::exception { 57 | public: 58 | explicit TypeException(const char* message) : msg_(message) {} 59 | explicit TypeException(const std::string& message) : msg_(message) {} 60 | ~TypeException() throw() {} 61 | const char* what() const throw() { 62 | return msg_.c_str(); 63 | } 64 | 65 | protected: 66 | std::string msg_; 67 | }; 68 | 69 | } // namespace RosMsgParser 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /src/ros_type.cpp: -------------------------------------------------------------------------------- 1 | /***** MIT License **** 2 | * 3 | * Copyright (c) 2016-2022 Davide Faconti 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in all 13 | * copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #include "rosx_introspection/ros_type.hpp" 25 | 26 | #include 27 | 28 | namespace RosMsgParser { 29 | 30 | ROSType::ROSType(const std::string& name) : _base_name(name) { 31 | int pos = -1; 32 | for (size_t i = 0; i < name.size(); i++) { 33 | if (name[i] == '/') { 34 | pos = i; 35 | break; 36 | } 37 | } 38 | 39 | if (pos == -1) { 40 | _msg_name = _base_name; 41 | } else { 42 | _pkg_name = std::string_view(_base_name.data(), pos); 43 | pos++; 44 | _msg_name = std::string_view(_base_name.data() + pos, _base_name.size() - pos); 45 | } 46 | 47 | _id = toBuiltinType(_msg_name); 48 | _hash = std::hash{}(_base_name); 49 | } 50 | 51 | ROSType& ROSType::operator=(const ROSType& other) { 52 | int pos = other._pkg_name.size(); 53 | _base_name = other._base_name; 54 | _pkg_name = std::string_view(_base_name.data(), pos); 55 | if (pos > 0) { 56 | pos++; 57 | } 58 | _msg_name = std::string_view(_base_name.data() + pos, _base_name.size() - pos); 59 | _id = other._id; 60 | _hash = other._hash; 61 | return *this; 62 | } 63 | 64 | ROSType& ROSType::operator=(ROSType&& other) { 65 | int pos = other._pkg_name.size(); 66 | _base_name = std::move(other._base_name); 67 | _pkg_name = std::string_view(_base_name.data(), pos); 68 | if (pos > 0) { 69 | pos++; 70 | } 71 | _msg_name = std::string_view(_base_name.data() + pos, _base_name.size() - pos); 72 | _id = other._id; 73 | _hash = other._hash; 74 | return *this; 75 | } 76 | 77 | void ROSType::setPkgName(std::string_view new_pkg) { 78 | assert(_pkg_name.size() == 0); 79 | 80 | size_t pos = new_pkg.size(); 81 | _base_name = std::string(new_pkg) + "/" + _base_name; 82 | 83 | _pkg_name = std::string_view(_base_name.data(), pos++); 84 | _msg_name = std::string_view(_base_name.data() + pos, _base_name.size() - pos); 85 | 86 | _hash = std::hash{}(_base_name); 87 | } 88 | 89 | } // namespace RosMsgParser 90 | -------------------------------------------------------------------------------- /include/rosx_introspection/ros_message.hpp: -------------------------------------------------------------------------------- 1 | /***** MIT License **** 2 | * 3 | * Copyright (c) 2016-2022 Davide Faconti 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in all 13 | * copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #ifndef ROS_INTROSPECTION_ROSMESSAGE_H 25 | #define ROS_INTROSPECTION_ROSMESSAGE_H 26 | 27 | #include 28 | 29 | #include "rosx_introspection/ros_field.hpp" 30 | #include "rosx_introspection/tree.hpp" 31 | 32 | namespace RosMsgParser { 33 | 34 | class ROSMessage { 35 | public: 36 | using Ptr = std::shared_ptr; 37 | 38 | /// This constructor does most of the work in terms of parsing. 39 | /// It uses the message definition to extract fields and types. 40 | ROSMessage(const std::string& msg_def); 41 | 42 | const ROSField& field(size_t i) const { 43 | return _fields[i]; 44 | } 45 | 46 | const std::vector& fields() const { 47 | return _fields; 48 | } 49 | 50 | std::vector& fields() { 51 | return _fields; 52 | } 53 | 54 | const ROSType& type() const { 55 | return _type; 56 | } 57 | 58 | void setType(const ROSType& new_type) { 59 | _type = new_type; 60 | } 61 | 62 | private: 63 | ROSType _type; 64 | std::vector _fields; 65 | }; 66 | 67 | typedef details::TreeNode FieldTreeNode; 68 | typedef details::Tree FieldTree; 69 | 70 | struct MessageSchema { 71 | using Ptr = std::shared_ptr; 72 | 73 | std::string topic_name; 74 | FieldTree field_tree; 75 | ROSMessage::Ptr root_msg; 76 | RosMessageLibrary msg_library; 77 | }; 78 | 79 | //------------------------------------------------ 80 | 81 | inline std::ostream& operator<<(std::ostream& os, const ROSMessage& msg) { 82 | os << msg.type(); 83 | return os; 84 | } 85 | 86 | inline std::ostream& operator<<(std::ostream& os, const ROSMessage* msg) { 87 | os << msg->type(); 88 | return os; 89 | } 90 | 91 | std::vector ParseMessageDefinitions(const std::string& multi_def, const ROSType& type); 92 | 93 | MessageSchema::Ptr BuildMessageSchema(const std::string& topic_name, const std::vector& parsed_msgs); 94 | 95 | } // namespace RosMsgParser 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /include/rosx_introspection/ros_field.hpp: -------------------------------------------------------------------------------- 1 | /***** MIT License **** 2 | * 3 | * Copyright (c) 2016-2022 Davide Faconti 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in all 13 | * copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #pragma once 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "rosx_introspection/ros_type.hpp" 33 | 34 | namespace RosMsgParser { 35 | 36 | class ROSMessage; 37 | 38 | using RosMessageLibrary = std::unordered_map>; 39 | 40 | class Parser; 41 | 42 | /** 43 | * @brief A ROSMessage will contain one or more ROSField(s). Each field is little more 44 | * than a name / type pair. 45 | */ 46 | class ROSField { 47 | public: 48 | ROSField(const ROSType& type, const std::string& name); 49 | 50 | ROSField(const std::string& definition); 51 | 52 | const std::string& name() const { 53 | return _fieldname; 54 | } 55 | 56 | const ROSType& type() const { 57 | return _type; 58 | } 59 | 60 | void changeType(const ROSType& type) { 61 | _type = type; 62 | } 63 | 64 | /// True if field is a constant in message definition 65 | bool isConstant() const { 66 | return _is_constant; 67 | } 68 | 69 | /// If constant, value of field, else undefined 70 | const std::string& value() const { 71 | return _value; 72 | } 73 | 74 | /// True if the type is an array 75 | bool isArray() const { 76 | return _is_array; 77 | } 78 | 79 | /// 1 if !is_array, -1 if is_array and array is 80 | /// variable length, otherwise length in name 81 | int arraySize() const { 82 | return _array_size; 83 | } 84 | 85 | friend class ROSMessage; 86 | 87 | std::shared_ptr getMessagePtr(const RosMessageLibrary& library) const; 88 | 89 | protected: 90 | std::string _fieldname; 91 | ROSType _type; 92 | std::string _value; 93 | bool _is_array; 94 | bool _is_constant = false; 95 | int _array_size; 96 | 97 | mutable const RosMessageLibrary* _cache_library = nullptr; 98 | mutable std::shared_ptr _cache_message; 99 | }; 100 | 101 | void TrimStringLeft(std::string& s); 102 | 103 | void TrimStringRight(std::string& s); 104 | 105 | void TrimString(std::string& s); 106 | 107 | } // namespace RosMsgParser 108 | -------------------------------------------------------------------------------- /include/rosx_introspection/deserializer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DESERIALIZER_HPP 2 | #define DESERIALIZER_HPP 3 | 4 | // API adapted to FastCDR 5 | 6 | #include 7 | 8 | #include "rosx_introspection/builtin_types.hpp" 9 | #include "rosx_introspection/variant.hpp" 10 | 11 | namespace nanocdr { 12 | class Decoder; 13 | } // namespace nanocdr 14 | 15 | namespace RosMsgParser { 16 | 17 | class Deserializer { 18 | public: 19 | virtual void init(Span buffer) { 20 | _buffer = buffer; 21 | reset(); 22 | } 23 | 24 | virtual bool isROS2() const = 0; 25 | 26 | virtual ~Deserializer() = default; 27 | 28 | // move the memory pointer 29 | virtual void jump(size_t bytes) = 0; 30 | 31 | // deserialize the current pointer into a variant (not a string) 32 | [[nodiscard]] virtual Variant deserialize(BuiltinType type) = 0; 33 | 34 | [[nodiscard]] virtual Span deserializeByteSequence() = 0; 35 | 36 | // deserialize the current pointer into a string 37 | virtual void deserializeString(std::string& out) = 0; 38 | 39 | [[nodiscard]] virtual uint32_t deserializeUInt32() = 0; 40 | 41 | [[nodiscard]] virtual const uint8_t* getCurrentPtr() const = 0; 42 | 43 | [[nodiscard]] virtual size_t bytesLeft() const { 44 | return _buffer.size() - (getCurrentPtr() - _buffer.data()); 45 | } 46 | 47 | // reset the pointer to beginning of buffer 48 | virtual void reset() = 0; 49 | 50 | protected: 51 | Span _buffer; 52 | }; 53 | 54 | //----------------------------------------------------------------- 55 | 56 | // Specialization od deserializer that works with ROS1 57 | class ROS_Deserializer : public Deserializer { 58 | public: 59 | Variant deserialize(BuiltinType type) override; 60 | 61 | bool isROS2() const override { 62 | return false; 63 | } 64 | 65 | void deserializeString(std::string& dst) override; 66 | 67 | uint32_t deserializeUInt32() override; 68 | 69 | Span deserializeByteSequence() override; 70 | 71 | const uint8_t* getCurrentPtr() const override; 72 | 73 | void jump(size_t bytes) override; 74 | 75 | void reset() override; 76 | 77 | protected: 78 | const uint8_t* _ptr; 79 | size_t _bytes_left; 80 | 81 | template 82 | T deserialize() { 83 | T out; 84 | if (sizeof(T) > _bytes_left) { 85 | throw std::runtime_error("Buffer overrun in Deserializer"); 86 | } 87 | out = (*(reinterpret_cast(_ptr))); 88 | _bytes_left -= sizeof(T); 89 | _ptr += sizeof(T); 90 | return out; 91 | } 92 | }; 93 | 94 | //----------------------------------------------------------------- 95 | 96 | // Specialization od deserializer that works with ROS2 97 | // wrapping FastCDR 98 | class NanoCDR_Deserializer : public Deserializer { 99 | public: 100 | Variant deserialize(BuiltinType type) override; 101 | 102 | void deserializeString(std::string& dst) override; 103 | 104 | uint32_t deserializeUInt32() override; 105 | 106 | Span deserializeByteSequence() override; 107 | 108 | const uint8_t* getCurrentPtr() const override; 109 | 110 | void jump(size_t bytes) override; 111 | 112 | virtual void reset() override; 113 | 114 | bool isROS2() const override { 115 | return true; 116 | } 117 | 118 | protected: 119 | std::shared_ptr _cdr_decoder; 120 | }; 121 | 122 | using ROS2_Deserializer = NanoCDR_Deserializer; 123 | 124 | } // namespace RosMsgParser 125 | 126 | #endif // DESERIALIZER_HPP 127 | -------------------------------------------------------------------------------- /include/rosx_introspection/ros_utils/ros2_helpers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "rosx_introspection/ros_field.hpp" 10 | 11 | namespace RosMsgParser { 12 | 13 | class RmwInterface { 14 | public: 15 | using RmwSerializedPtr = std::shared_ptr; 16 | 17 | RmwInterface(); 18 | ~RmwInterface() = default; 19 | 20 | template 21 | RmwSerializedPtr serialize_message(const T& message, const rosidl_message_type_support_t* type_support) { 22 | auto serialized_message = get_initialized_serialized_message(0); 23 | auto error = rmw_serialize(&message, type_support, serialized_message.get()); 24 | if (error != RCL_RET_OK) { 25 | throw std::runtime_error("Failed to serialize"); 26 | } 27 | return serialized_message; 28 | } 29 | 30 | template 31 | std::shared_ptr deserialize_message( 32 | RmwSerializedPtr serialized_msg, const rosidl_message_type_support_t* type_support) { 33 | auto message = std::make_shared(); 34 | auto error = rmw_deserialize( 35 | serialized_msg.get(), 36 | // get_message_typesupport(message), 37 | type_support, message.get()); 38 | if (error != RCL_RET_OK) { 39 | RCUTILS_LOG_ERROR_NAMED("rosbag2_test_common", "Leaking memory. Error: %s", rcutils_get_error_string().str); 40 | } 41 | return message; 42 | } 43 | 44 | private: 45 | RmwSerializedPtr get_initialized_serialized_message(size_t capacity); 46 | rcutils_allocator_t rcutils_allocator_; 47 | }; 48 | 49 | std::string GetMessageDefinition(const std::string& datatype); 50 | 51 | template 52 | inline std::vector BuildMessageBuffer(const T& msg, const std::string& topic_type) { 53 | const auto& ts_identifier = rosidl_typesupport_cpp::typesupport_identifier; 54 | auto ts_library = rosbag2_cpp::get_typesupport_library(topic_type, ts_identifier); 55 | auto typesupport = rosbag2_cpp::get_typesupport_handle(topic_type, ts_identifier, ts_library); 56 | 57 | RmwInterface rmw; 58 | auto serialized_msg = rmw.serialize_message(msg, typesupport); 59 | std::vector buffer(serialized_msg->buffer_length, 0); 60 | memcpy(buffer.data(), serialized_msg->buffer, serialized_msg->buffer_length); 61 | return buffer; 62 | } 63 | 64 | template 65 | inline T BufferToMessage(const void* bufferPtr, size_t bufferSize) { 66 | // get the type name of the ROS message with traits 67 | const std::string topic_type = rosidl_generator_traits::name(); 68 | const auto& ts_identifier = rosidl_typesupport_cpp::typesupport_identifier; 69 | auto ts_library = rosbag2_cpp::get_typesupport_library(topic_type, ts_identifier); 70 | auto type_support = rosbag2_cpp::get_typesupport_handle(topic_type, ts_identifier, ts_library); 71 | 72 | rmw_serialized_message_t serialized_msg; 73 | serialized_msg.buffer_capacity = bufferSize; 74 | serialized_msg.buffer_length = bufferSize; 75 | serialized_msg.buffer = const_cast(reinterpret_cast(bufferPtr)); 76 | 77 | T message; 78 | auto res = rmw_deserialize(&serialized_msg, type_support, &message); 79 | return message; 80 | } 81 | 82 | template 83 | inline T BufferToMessage(const std::vector& buffer) { 84 | return BufferToMessage(buffer.data(), buffer.size()); 85 | } 86 | 87 | } // namespace RosMsgParser 88 | -------------------------------------------------------------------------------- /include/rosx_introspection/stringtree_leaf.hpp: -------------------------------------------------------------------------------- 1 | /***** MIT License **** 2 | * 3 | * Copyright (c) 2016-2022 Davide Faconti 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy 6 | * of this software and associated documentation files (the "Software"), to deal 7 | * in the Software without restriction, including without limitation the rights 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | * copies of the Software, and to permit persons to whom the Software is 10 | * furnished to do so, subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in all 13 | * copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | * SOFTWARE. 22 | */ 23 | 24 | #ifndef ROS_INTROSPECTION_FieldTreeLeaf_H 25 | #define ROS_INTROSPECTION_FieldTreeLeaf_H 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include "rosx_introspection/ros_message.hpp" 32 | 33 | // Brutally faster for numbers below 100 34 | inline int print_number(char* buffer, uint16_t value) { 35 | const char DIGITS[] = 36 | "00010203040506070809" 37 | "10111213141516171819" 38 | "20212223242526272829" 39 | "30313233343536373839" 40 | "40414243444546474849" 41 | "50515253545556575859" 42 | "60616263646566676869" 43 | "70717273747576777879" 44 | "80818283848586878889" 45 | "90919293949596979899"; 46 | if (value < 10) { 47 | buffer[0] = static_cast('0' + value); 48 | return 1; 49 | } else if (value < 100) { 50 | value *= 2; 51 | buffer[0] = DIGITS[value]; 52 | buffer[1] = DIGITS[value + 1]; 53 | return 2; 54 | } else { 55 | return snprintf(buffer, 16, "%d", value); 56 | } 57 | } 58 | 59 | namespace RosMsgParser { 60 | 61 | /** 62 | * @brief The FieldTreeLeaf is, as the name suggests, a leaf (terminal node) 63 | * of a StringTree. 64 | * It provides the pointer to the node and a list of numbers that represent 65 | * the index that corresponds to the placeholder "#". 66 | * 67 | * For example if you want to represent the string 68 | * 69 | * foo/2/bar/3/hello/world 70 | * 71 | * This would correspond to a branch of the tree (from root to the leaf) equal to these 6 72 | * nodes, where "foo" is the root and "world" is the leaf 73 | * 74 | * foo -> # -> bar -> # ->hello -> world 75 | * 76 | * array_size will be equal to two and index_array will contain these numbers {2,3} 77 | * 78 | */ 79 | struct FieldLeaf { 80 | const FieldTreeNode* node; 81 | SmallVector index_array; 82 | }; 83 | 84 | struct FieldsVector { 85 | FieldsVector() = default; 86 | 87 | FieldsVector(const FieldLeaf& leaf); 88 | 89 | SmallVector fields; 90 | SmallVector index_array; 91 | 92 | /// Utility functions to print the entire branch 93 | void toStr(std::string& destination) const; 94 | 95 | std::string toStdString() const { 96 | std::string out; 97 | toStr(out); 98 | return out; 99 | } 100 | }; 101 | 102 | //--------------------------------- 103 | 104 | inline std::ostream& operator<<(std::ostream& os, const FieldsVector& leaf) { 105 | std::string dest; 106 | leaf.toStr(dest); 107 | os << dest; 108 | return os; 109 | } 110 | 111 | } // namespace RosMsgParser 112 | 113 | #endif // ROSTYPE_H 114 | -------------------------------------------------------------------------------- /test/test_ros1.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "doctest.h" 5 | #include "rosx_introspection/ros_parser.hpp" 6 | #include "rosx_introspection/ros_utils/ros1_helpers.hpp" 7 | 8 | using namespace RosMsgParser; 9 | 10 | TEST_CASE("Parse ROS1 [JointState]") { 11 | ParsersCollection parser; 12 | parser.registerParser( 13 | "joint_state", GetMessageType(), GetMessageDefinition()); 14 | 15 | sensor_msgs::JointState joint_state; 16 | 17 | joint_state.header.seq = 2016; 18 | joint_state.header.stamp.sec = 1234; 19 | joint_state.header.stamp.nsec = 567 * 1000 * 1000; 20 | joint_state.header.frame_id = "pippo"; 21 | 22 | joint_state.name.resize(3); 23 | joint_state.position.resize(3); 24 | joint_state.velocity.resize(3); 25 | joint_state.effort.resize(3); 26 | 27 | std::string names[3]; 28 | names[0] = ("hola"); 29 | names[1] = ("ciao"); 30 | names[2] = ("bye"); 31 | 32 | for (int i = 0; i < 3; i++) { 33 | joint_state.name[i] = names[i]; 34 | joint_state.position[i] = 10 + i; 35 | joint_state.velocity[i] = 30 + i; 36 | joint_state.effort[i] = 50 + i; 37 | } 38 | 39 | std::vector buffer = BuildMessageBuffer(joint_state); 40 | 41 | auto flat_container = parser.deserialize("joint_state", Span(buffer)); 42 | 43 | CHECK(flat_container->value[0].first.toStdString() == ("joint_state/header/seq")); 44 | CHECK(flat_container->value[0].second.convert() == 2016); 45 | 46 | CHECK(flat_container->value[1].first.toStdString() == ("joint_state/header/stamp")); 47 | CHECK(flat_container->value[1].second.convert() == 1234.567); 48 | auto time = flat_container->value[1].second.convert