├── .clang-format ├── .github └── workflows │ └── tests.yml ├── .gitignore ├── CHANGELOG.md ├── CMakeLists.txt ├── LICENSE ├── README.md ├── README_CN.md ├── build.sh ├── cmake ├── git_version.cmake ├── googletest.cmake └── policy.cmake ├── docs ├── comparison_with_px4_uorb.md ├── getting_started.md └── getting_started_cn.md ├── examples ├── CMakeLists.txt ├── c_pub_sub │ ├── CMakeLists.txt │ └── c_pub_sub.c ├── common │ └── slog.h ├── cpp_pub_sub │ ├── CMakeLists.txt │ └── cpp_pub_sub.cc ├── msg │ ├── .gitignore │ ├── CMakeLists.txt │ ├── example_string.msg │ ├── msg_template.msg │ ├── sensor_accel.msg │ └── sensor_gyro.msg └── tcp_topic_listener │ ├── CMakeLists.txt │ ├── README.md │ ├── tcp_topic_listener_test.cc │ └── uorb_tcp_client.sh ├── include └── uorb │ ├── abs_time.h │ ├── internal │ └── noncopyable.h │ ├── publication.h │ ├── publication_multi.h │ ├── subscription.h │ ├── subscription_interval.h │ └── uorb.h ├── src ├── base │ ├── atomic.h │ ├── condition_variable.h │ ├── condition_variable_test.cc │ ├── intrusive_list.h │ ├── mutex.h │ ├── orb_errno.cc │ ├── orb_errno.h │ └── rw_mutex.h ├── callback.h ├── device_master.cc ├── device_master.h ├── device_node.cc ├── device_node.h ├── git_version.cc.in ├── subscription_impl.h └── uorb.cc ├── tests ├── CMakeLists.txt ├── msg │ ├── .gitignore │ ├── CMakeLists.txt │ ├── orb_test.msg │ ├── orb_test_large.msg │ ├── orb_test_medium.msg │ └── orb_test_queue_poll.msg ├── slog.h ├── uorb_unit_test.cc └── uorb_unit_test.h ├── toolchains ├── himix200.toolchain.cmake ├── hisiv500.toolchain.cmake └── host.gcc.toolchain.cmake └── tools ├── msg ├── templates │ └── uorb │ │ ├── msg.cc.em │ │ ├── msg.h.em │ │ ├── uorb_topics.cc.em │ │ └── uorb_topics.h.em └── tools │ ├── em.py │ ├── genmsg │ ├── __init__.py │ ├── base.py │ ├── command_line.py │ ├── deps.py │ ├── gentools.py │ ├── msg_loader.py │ ├── msgs.py │ ├── names.py │ ├── srvs.py │ └── template_tools.py │ ├── packaging │ ├── __about__.py │ ├── __init__.py │ ├── _manylinux.py │ ├── _musllinux.py │ ├── _structures.py │ ├── markers.py │ ├── py.typed │ ├── requirements.py │ ├── specifiers.py │ ├── tags.py │ ├── utils.py │ └── version.py │ ├── px_generate_uorb_topic_files.py │ ├── px_generate_uorb_topic_helper.py │ └── requirements.txt └── uorb_tcp_topic_listener_lib ├── CMakeLists.txt ├── include └── uorb_tcp_listener.h └── src ├── command_manager.h ├── data_printer.h ├── fd_stream.h ├── string_helper.h ├── tcp_server.h ├── uorb_tcp_listener.cc └── var_attr.h /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | -------------------------------------------------------------------------------- /.github/workflows/tests.yml: -------------------------------------------------------------------------------- 1 | name: CMake 2 | 3 | on: 4 | push: 5 | branches: [ "main" ] 6 | pull_request: 7 | branches: [ "main" ] 8 | 9 | env: 10 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 11 | BUILD_TYPE: Release 12 | 13 | jobs: 14 | test: 15 | name: ${{ matrix.os }}.${{ matrix.compiler.name }} 16 | # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. 17 | # You can convert this to a matrix build if you need cross-platform coverage. 18 | # See: https://docs.github.com/en/actions/using-jobs/using-a-matrix-for-your-jobs#using-a-matrix-strategy 19 | strategy: 20 | matrix: 21 | os: [ ubuntu-latest, macos-latest ] 22 | compiler: 23 | - { name: GNU, CC: gcc, CXX: g++, GCOV: gcov } 24 | - { name: LLVM, CC: clang, CXX: clang++ } 25 | runs-on: ${{ matrix.os }} 26 | 27 | steps: 28 | - uses: actions/checkout@v3 29 | 30 | - name: Configure CMake 31 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 32 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 33 | run: 'cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DUORB_BUILD_TESTS=ON 34 | -DCMAKE_C_COMPILER=${{matrix.compiler.CC}} -DCMAKE_CXX_COMPILER=${{matrix.compiler.CXX}}' 35 | 36 | - name: Build 37 | # Build your program with the given configuration 38 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 39 | 40 | - name: Test 41 | working-directory: ${{github.workspace}}/build 42 | # Execute tests defined by the CMake configuration. 43 | # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail 44 | run: ctest -C ${{env.BUILD_TYPE}} --extra-verbose 45 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | All notable changes to this project will be documented in this file. 4 | 5 | The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), and this project adheres 6 | to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). 7 | 8 | ## [Unreleased] 9 | 10 | [Unreleased]: https://github.com/ShawnFeng0/uorb/compare/v0.3.1...HEAD 11 | 12 | ## [0.3.1] - 2025-06-01 13 | [0.3.1]: https://github.com/ShawnFeng0/uorb/compare/v0.3.0...v0.3.1 14 | 15 | ### Fixed 16 | 17 | - Improve semaphore acquisition logic based on timeout value 18 | 19 | ## [0.3.0] - 2023-06-13 20 | [0.3.0]: https://github.com/ShawnFeng0/uorb/compare/v0.2.3...v0.3.0 21 | 22 | ### Added 23 | 24 | - Support custom uorb listener port 25 | - Add ORB_QUEUE_SIZE to the orb_metadata structure 26 | - Automatically configures the queue according to the topic's metadata 27 | - Support macos platform 28 | - Make ConditionVariable support macos platform 29 | - Add unit test for ConditionVariable 30 | - Remove the functional interface of the wait_until class in ConditionVariable, which is not actually a commonly used interface 31 | 32 | ### Changed 33 | 34 | - Make the tcp listener library use the uorb::listener namespace 35 | - ORB_ID(name) uses a global pointer variable instead of a global function 36 | - msg_gen: Add dependent python libraries to reduce usage complexity 37 | - compile: Simplify cmake scripts 38 | - Add cmake policy configuration file: cmake/policy.cmake 39 | - Add CMP0135 cmake strategy to prevent warning when downloading google test library 40 | - Add googletest download script: cmake/googletest.cmake 41 | - Add a cmake macro to get the git version: cmake/git_version.cmake 42 | 43 | ### Fixed 44 | 45 | - topic_listener: when NDEBUG is defined, the judgment in assert will be optimized away 46 | 47 | ## [0.2.3] - 2021-10-10 48 | [0.2.3]: https://github.com/ShawnFeng0/uorb/compare/v0.2.2...v0.2.3 49 | 50 | ### Added 51 | 52 | - Add `orb_get_topic_status()` interface to get topics status, such as the number of publish subscribers, publish index, etc. 53 | - [optional] Add topics tcp listener library. The tcp client (such as `netcat`) can be used outside the application to connect to the topics listener server to obtain the status of uorb's topics, especially the topic data (10HZ) can be obtained in real time. 54 | 55 | ## [0.2.2] - 2021-09-09 56 | 57 | [0.2.2]: https://github.com/ShawnFeng0/uorb/compare/v0.2.1...v0.2.2 58 | 59 | ### Fixed 60 | 61 | - orb_poll is invalid when using -1 time to wait (64bit system) 62 | - An anonymous publisher alone cannot publish a message 63 | - Make the parameters of the orb_elapsed_time_us function only use values instead of pointers 64 | - Subscription_interval.h compile error 65 | - To build using modern cmake syntax, only target_link_libraries(xxx uorb) is required, and the header file path is no 66 | longer required. 67 | - The number of publishers is incorrect 68 | - When the queue length is 1, the subscriber index is not updated to the latest in time 69 | 70 | ### Added 71 | 72 | - Add time constant literals (_s, _ms, _us) 73 | - Add MIT license 74 | 75 | ### Removed 76 | 77 | - Delete unused file: base/semaphore.h 78 | 79 | ## [0.2.1] - 2021-02-28 80 | 81 | [0.2.1]: https://github.com/ShawnFeng0/uorb/compare/v0.2.0...v0.2.1 82 | 83 | ### Fixed 84 | 85 | - Crash when orb_poll was used for multiple subscriptions 86 | 87 | ## [0.2.0] - 2021-02-28 88 | 89 | [0.2.0]: https://github.com/ShawnFeng0/uorb/compare/v0.1.0...v0.2.0 90 | 91 | ### Added 92 | 93 | - Add interfaces for anonymously publishing and copying topic data 94 | - Add Chinese version of README 95 | - Add DeviceNode::publisher_count_ statistics 96 | - uORB::Publication*: template parameter automatically obtains the queue size according to the type 97 | 98 | ## [0.1.0] - 2020-10-9 99 | 100 | [0.1.0]: https://github.com/ShawnFeng0/uorb/releases/tag/v0.1.0 101 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12...4.0) 2 | 3 | # Set cmake_policies 4 | include(cmake/policy.cmake) 5 | use_cmake_policy() 6 | 7 | project(uorb) 8 | 9 | set(CMAKE_CXX_STANDARD 11) 10 | 11 | option(UORB_BUILD_EXAMPLES "Build examples" OFF) 12 | option(UORB_BUILD_TESTS "Build tests" OFF) 13 | 14 | # Generate git version info 15 | include(cmake/git_version.cmake) 16 | set_git_version(UORB_GIT_TAG) 17 | message(STATUS "uORB version: ${UORB_GIT_TAG}") 18 | 19 | configure_file( 20 | "src/git_version.cc.in" 21 | "src/git_version.cc" 22 | ) 23 | 24 | add_compile_options(-Wextra -Wall) 25 | 26 | # uorb library 27 | add_library(uorb) 28 | target_sources(uorb PRIVATE 29 | src/base/orb_errno.cc 30 | src/device_master.cc 31 | src/device_node.cc 32 | src/uorb.cc 33 | ${CMAKE_CURRENT_BINARY_DIR}/src/git_version.cc 34 | ) 35 | target_include_directories(uorb PUBLIC include) 36 | target_include_directories(uorb PRIVATE src) 37 | target_link_libraries(uorb PRIVATE pthread) 38 | 39 | add_subdirectory(tools/uorb_tcp_topic_listener_lib EXCLUDE_FROM_ALL) 40 | 41 | # install uorb 42 | install(TARGETS uorb 43 | ARCHIVE DESTINATION lib) 44 | install(DIRECTORY include/uorb DESTINATION include) 45 | 46 | if (UORB_BUILD_EXAMPLES) 47 | add_subdirectory(examples) 48 | endif () 49 | 50 | if (UORB_BUILD_TESTS) 51 | include(cmake/googletest.cmake) 52 | enable_testing() 53 | 54 | # For uorb internal unit test 55 | add_executable(uorb_unittest src/base/condition_variable_test.cc) 56 | target_link_libraries(uorb_unittest PRIVATE uorb GTest::gtest_main) 57 | target_include_directories(uorb_unittest PRIVATE src) 58 | add_test(uorb_unittest uorb_unittest) 59 | 60 | # For uorb library interface test 61 | add_subdirectory(tests) 62 | endif () 63 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Shawn Feng 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # uORB 2 | 3 | ![Tests](https://github.com/ShawnFeng0/uorb/actions/workflows/tests.yml/badge.svg) 4 | 5 | English | [中文](README_CN.md) 6 | 7 | The uORB is an asynchronous publish() / subscribe() messaging API used for inter-thread communication. "UORB" is the 8 | abbreviation of Micro Object Request Broker. 9 | 10 | uORB was originally a messaging middleware in PX4 Autopilot, run on Nuttx rtos or Linux: 11 | 12 | * [Introduction of uORB in PX4](https://dev.px4.io/master/en/middleware/uorb.html) 13 | 14 | The project was re-implemented based on POSIX based on the original API, some redundant software layers were removed, 15 | and only most of the core functions were retained. Some of these improvements and fixes have been merged upstream of 16 | PX4. 17 | 18 | * [Comparison with uORB of PX4](docs/comparison_with_px4_uorb.md) 19 | 20 | ## Features 21 | 22 | * Based on POSIX, good compatibility 23 | * Real-time response mechanism similar to poll() function 24 | * A flexible and easy-to-use C++ interface, which can operate topics like local structures 25 | 26 | ## Dependencies 27 | 28 | Compiling the uorb library requires the support of c++11, and most compilations are currently supported. 29 | 30 | We have a message generator that can easily generate message meta-data. These libraries are needed to make it work: 31 | 32 | ```shell 33 | pip3 install -r tools/msg/tools/requirements.txt 34 | ``` 35 | 36 | ## Documentation 37 | 38 | * [Getting Started Guide](docs/getting_started.md) 39 | * API reference (TODO) 40 | * [Changelog](CHANGELOG.md) 41 | 42 | ## Examples 43 | 44 | [uorb-examples](https://github.com/ShawnFeng0/uorb-examples.git) 45 | 46 | ## Tools 47 | 48 | ### uorb topic listener 49 | 50 | uorb also has a [topic listener library](tools/uorb_tcp_topic_listener_lib). It is responsible for starting a tcp server, which is convenient for developers to monitor uorb topic data in real time outside the process. 51 | 52 | Here is an [example](examples/tcp_topic_listener) of using this listener. 53 | -------------------------------------------------------------------------------- /README_CN.md: -------------------------------------------------------------------------------- 1 | # uORB 2 | 3 | ![Tests](https://github.com/ShawnFeng0/uorb/actions/workflows/tests.yml/badge.svg) 4 | 5 | uORB是用于线程间通信的消息发布/订阅的库。"UORB"是Micro Object Request Broker的缩写。 6 | 7 | uORB源自PX4开源飞控的消息中间件，运行在Nuttx rtos和Linux上： 8 | 9 | * [PX4中的uORB介绍](https://dev.px4.io/master/en/middleware/uorb.html) 10 | 11 | 这个项目重新在原始API的上基于POSIX重新实现了，去除了一些冗余的软件层，并且保留了大部分核心功能。 12 | 其中一些改进和修复已经合并到PX4上游。 13 | 14 | * [与PX4的uORB的对比](docs/comparison_with_px4_uorb.md) 15 | 16 | ## 特性 17 | 18 | * 基于POSIX，有良好的兼容性 19 | * 类似于poll()函数的实时响应机制 20 | * 灵活和易用的C++接口，就像操作本地数据一样操作订阅数据 21 | 22 | ## 依赖关系 23 | 24 | 编译uORB库需要C++11的支持，目前大多数编译器都支持。 25 | 26 | 需要一个消息生成器用来生成消息元数据，需要这些python库才能工作： 27 | 28 | ```shell 29 | pip3 install -r tools/msg/tools/requirements.txt 30 | ``` 31 | 32 | ## 文档 33 | 34 | * [入门指导](docs/getting_started_cn.md) 35 | * API 参考 (TODO) 36 | * [变更日志](CHANGELOG.md) 37 | 38 | ## 示例 39 | 40 | [uorb-examples](https://github.com/ShawnFeng0/uorb-examples.git) 41 | 42 | ## 工具 43 | 44 | ### uorb 主题监听器 45 | 46 | uorb 还有一个[话题监听器库](tools/uorb_tcp_topic_listener_lib)。 它负责启动一个tcp服务器，方便开发者在进程外实时监控uorb话题数据。 47 | 48 | 这里有一个使用监听器的[示例](examples/tcp_topic_listener)。 49 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | 3 | SOURCE_DIR=$( 4 | cd "$(dirname $0)" || exit 5 | pwd 6 | ) 7 | 8 | # Get the platform name from the first parameter 9 | if [ $# -ge 1 ]; then 10 | PLATFORM_NAME=$1 11 | else 12 | echo "USAGE: $0 [platform_name]" 13 | fi 14 | 15 | # Get platform toolchain info 16 | PLATFORM_NAME=${PLATFORM_NAME:-host.gcc} 17 | PLATFORM_TOOLCHAIN_FILE=$SOURCE_DIR/toolchains/$PLATFORM_NAME.toolchain.cmake 18 | 19 | # Check if the platform toolchain exists 20 | if [ ! -f $PLATFORM_TOOLCHAIN_FILE ]; then 21 | echo "Can't find platform toolchain file: $PLATFORM_TOOLCHAIN_FILE" 22 | exit 1 23 | fi 24 | 25 | # Build library 26 | set -x 27 | rm -rf $SOURCE_DIR/cmake-build-$PLATFORM_NAME 28 | mkdir -p "$SOURCE_DIR/cmake-build-$PLATFORM_NAME" && 29 | cd "$SOURCE_DIR/cmake-build-$PLATFORM_NAME" && 30 | cmake \ 31 | -DCMAKE_TOOLCHAIN_FILE="$PLATFORM_TOOLCHAIN_FILE" \ 32 | -DCMAKE_EXPORT_COMPILE_COMMANDS=ON \ 33 | .. && 34 | make -j && 35 | make install 36 | -------------------------------------------------------------------------------- /cmake/git_version.cmake: -------------------------------------------------------------------------------- 1 | macro(set_git_version version) 2 | # Generate git version info 3 | if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/.git) 4 | execute_process( 5 | COMMAND git describe --always --tags --long --dirty=+ 6 | OUTPUT_VARIABLE ${version} 7 | OUTPUT_STRIP_TRAILING_WHITESPACE 8 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 9 | ) 10 | else () 11 | set(${version} "v0.0.0-0-not_git_repo") 12 | endif () 13 | endmacro() 14 | -------------------------------------------------------------------------------- /cmake/googletest.cmake: -------------------------------------------------------------------------------- 1 | # Google Test 2 | find_package(GTest) 3 | if (NOT GTest_FOUND) 4 | include(FetchContent) 5 | FetchContent_Declare( 6 | googletest 7 | URL https://github.com/google/googletest/archive/refs/tags/v1.13.0.zip 8 | ) 9 | # For Windows: Prevent overriding the parent project's compiler/linker settings 10 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) 11 | FetchContent_MakeAvailable(googletest) 12 | endif () 13 | -------------------------------------------------------------------------------- /cmake/policy.cmake: -------------------------------------------------------------------------------- 1 | # copy from https://github.com/sotatek-dev/solidity/blob/develop/cmake/EthPolicy.cmake 2 | 3 | # it must be a macro cause policies have scopes 4 | # http://www.cmake.org/cmake/help/v3.0/command/cmake_policy.html 5 | macro (use_cmake_policy) 6 | # Avoid warning about DOWNLOAD_EXTRACT_TIMESTAMP in CMake 3.24: 7 | if (CMAKE_VERSION VERSION_GREATER_EQUAL "3.24.0") 8 | cmake_policy(SET CMP0135 NEW) 9 | endif() 10 | endmacro() 11 | -------------------------------------------------------------------------------- /docs/comparison_with_px4_uorb.md: -------------------------------------------------------------------------------- 1 | # Difference from uORB of PX4 Autopilot 2 | 3 | The main difference lies in the implementation of the bottom layer, and the application program interface changes relatively little. It also passed the uORB unit test of PX4 Autopilot. 4 | 5 | ## API interface difference(v1.11) 6 | 7 | * Unified publication and subscription type: `orb_publication_t*` and `orb_subscription_t*` 8 | * Pass pointers when unpublishing and unsubscribing to avoid wild pointers(Reference from zmq) 9 | * Use ``bool`` type (include in ````) to indicate whether the operation is successful 10 | * Add independent ``orb_poll`` function, instead of ``px4_poll`` in PX4 Autopilot 11 | * Configure the topic's queue size in the topic's metadata, not at the time of publishing, which is good for a single topic with multiple publishers. 12 | 13 | | PX4 uORB | Current uORB | 14 | | :----------------------------------------------------------- | :----------------------------------------------------------- | 15 | | ~~orb_advert_t orb_advertise(const struct orb_metadata \*meta, const void \*data)~~ | | 16 | | **orb_advert_t** orb_advertise_queue(const struct orb_metadata \*meta, const void \*data~~, unsigned int queue_size~~) | **orb_publication_t** \*orb_create_publication(const struct orb_metadata \*meta) | 17 | | ~~orb_advert_t orb_advertise_multi(const struct orb_metadata \*meta, const void \*data, int \*instance)~~ | | 18 | | **orb_advert_t** orb_advertise_multi_queue(const struct orb_metadata \*meta, ~~const void \*data~~, int \*instance~~, unsigned queue_size~~) | **orb_publication_t** \*orb_create_publication_multi(const struct orb_metadata \*meta, unsigned int \*instance) | 19 | | **int** orb_publish(~~const struct orb_metadata \*meta,~~ orb_advert_t handle, const void \*data) | **bool** orb_publish(**orb_publication_t \*handle**, const void *data) | 20 | | **int** orb_unadvertise(orb_advert_t **handle**) | **bool** orb_destroy_publication(**orb_publication_t \*\*handle_ptr**) | 21 | | **int** orb_subscribe(const struct orb_metadata \*meta) | **orb_subscription_t ***orb_create_subscription(const struct orb_metadata *meta) | 22 | | **int** orb_subscribe_multi(const struct orb_metadata \*meta, unsigned instance) | **orb_subscription_t ***orb_create_subscription_multi(const struct orb_metadata *meta, unsigned instance) | 23 | | **int** orb_unsubscribe(**int handle**) | bool orb_destroy_subscription(**orb_subscription_t handle_ptr\*\***) | 24 | | **int** orb_copy(~~const struct orb_metadata \*meta,~~ **int handle**, void \*buffer) | **bool** orb_copy(**orb_subscription_t *handle**, void *buffer) | 25 | | **int** orb_check(**int handle** ~~, bool \*updated~~) | **bool** orb_check_update(**orb_subscription_t *handle**) | 26 | | ~~int orb_set_interval(int handle, unsigned interval)~~ | | 27 | | ~~int orb_get_interval(int handle, unsigned \*interval)~~ | | 28 | | int px4_poll(**px4_pollfd_struct_t** \*fds, unsigned int nfds, int timeout) | int orb_poll(**struct orb_pollfd** \*fds, unsigned int nfds, int timeout_ms) | 29 | 30 | ## Difference in implementation 31 | 32 | * Delete the ``o_id ``(``ORB_ID enum``) field in ``orb_metadata``. This field has no meaning and is very redundant. It is newly added in PX4-v1.11. 33 | * In C++, ``orb_metadata`` can be obtained directly through the orb message type, which can simplify the constructors of ``PublicationData`` and ``SubscriptionData``, and there is no need to pass in parameters such as ``ORB_ID(msg_name)`` to simplify the code and avoid errors. 34 | -------------------------------------------------------------------------------- /docs/getting_started.md: -------------------------------------------------------------------------------- 1 | # Add uORB to your project 2 | 3 | There are two ways use uORB in your project: 4 | 5 | * [**Use the uorb pre-built library binaries and headers**](#use-the-uorb-pre-built-library). Use this approach if you just want to use a stable version of the uorb library in your project. 6 | 7 | * [**Build uorb from source**](#build-uorb-from-source). Use this approach if you would like to debug or want the latest modifications. 8 | 9 | ## Use the uORB pre-built library 10 | 11 | It is recommended that you build uORB outside of the source directory, to avoid having issues with CMake generated files polluting the source directory: 12 | 13 | ```shell 14 | mkdir uorb-build 15 | cd uorb-build 16 | cmake -DCMAKE_C_COMPILER=clang \ 17 | -DCMAKE_CXX_COMPILER=clang++ \ 18 | -DCMAKE_INSTALL_PREFIX=./install \ 19 | $HOME/uorb # we're assuming this is where uorb is. 20 | make 21 | make install 22 | ``` 23 | 24 | The `./install` directory contains a lib directory and an include directory, link the libraries in the lib directory, and then add the include folder to the include path of your project. 25 | 26 | Or refer to the x.toolchain.cmake file in the toolchain directory to write a cmake toolchain file for your corresponding platform. 27 | 28 | ## Build uORB from source 29 | 30 | The best way to build from source code is as a subdirectory of the cmake project: 31 | 32 | Add the source code to your project. 33 | 34 | ```shell 35 | git submodule add https://github.com/ShawnFeng0/uorb.git # If you don't use git, just download the source code and unzip it. 36 | ``` 37 | 38 | Link the uORB library in the CMakeLists.txt of your own project. 39 | 40 | ```cmake 41 | # uORB library 42 | add_subdirectory(uorb) 43 | target_link_libraries(${PROJECT_NAME} PRIVATE uorb) # Link the uorb library and add the include path of uorb 44 | ``` 45 | 46 | This is an example of using this method (**recommended**): [uorb-examples](https://github.com/ShawnFeng0/uorb-examples.git). 47 | 48 | # Using uORB 49 | 50 | uORB communicates based on topics, not data streams, and each topic has a corresponding structure. 51 | 52 | The C interface of uORB is defined in uorb/uorb.h. 53 | 54 | ## Create uORB topic 55 | 56 | The topic is defined by the following macros and needs to include the topic name and the structure corresponding to the topic. 57 | 58 | ```C++ 59 | // uorb/uorb.h 60 | // To declare a topic, usually declare a topic in the header file. 61 | #define ORB_DECLARE(_name, _struct) ... 62 | ``` 63 | 64 | ```C++ 65 | // uorb/uorb.h 66 | // Define a topic, usually define a topic in the source file, can only be defined once and need to be compiled in the C++ source file (*.cpp/*.cc) 67 | #define ORB_SIMPLE_DEFINE(_name, _struct) ... 68 | ``` 69 | 70 | ### Use tools to manage uORB topics 71 | 72 | In order to manage and modify topics in a unified way, we use a message generation tool, which uses a way similar to ROS to define topics. 73 | 74 | I am lazy : ). I am not familiar with the code generation tool at the moment, so the generation tool is still using the official PX4 (may be cut later), but the uORB topic template has been modified. 75 | 76 | #### Add uORB topics 77 | 78 | First, create a folder to store uORB topics (the original topics are defined using .msg files, and it will generate *.h/*.cc topic files). Then add a topic file. 79 | 80 | ``` 81 | # msg/example_string.msg 82 | 83 | uint64 timestamp # time since system start (microseconds) 84 | 85 | uint32 STRING_LENGTH = 128 86 | int8[128] string 87 | ``` 88 | 89 | For the format of msg file, please refer to ros and PX4 or the examples in this project. 90 | 91 | Use the message generation tool in the `tools/msg` directory of this project to generate `*.h` and `*.cc` topic source files from `*.msg` files. There are many tool parameters. It is currently recommended to refer to the [examples](../examples) directory of this project or the independent [uorb examples](https://github.com/ShawnFeng0/uorb-examples.git) project `msg/CMakeLists.txt` realizes message generation. 92 | 93 | It should be noted that the address of the setting generation tool path in msg/CMakeLists.txt must be correct. 94 | 95 | ```Cmake 96 | # Need to specify the path of generator and message template, they are in the tools/msg directory of this project. 97 | set(tools_root ${CMAKE_CURRENT_SOURCE_DIR}/../../tools/msg) 98 | ``` 99 | 100 | #### Add uORB topic as a dependency 101 | 102 | Add the following statement to `CMakeLists.txt` at the level of the msg directory you created to use the topic library as a dependency. 103 | 104 | ```cmake 105 | # Generate orb message 106 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/msg) 107 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/msg) 108 | 109 | # The name of your topic library. It is a library generated after compiling all [topic].cc files. Please refer to the example implementation for details. 110 | link_libraries(uorb_examples_msgs) 111 | ``` 112 | 113 | ### Manually manage uORB topics 114 | 115 | Although it is not recommended, we also support directly using macros to manually define topics, which may be good for beginners. You need to make sure to declare `ORB_DECLARE` in the topic header file, and define `ORB_SIMPLE_DEFINE` in the `C++` source file. 116 | 117 | #### Declare uORB topics 118 | 119 | Just include the `uorb/uorb.h` header file in your topic header file and declare the topic directly. 120 | 121 | ```C++ 122 | // orb_topic.h 123 | #pragma once 124 | 125 | #include "uorb/uorb.h" 126 | 127 | struct orb_example_struct_str { 128 | char str[30]; 129 | int32_t index; 130 | }; 131 | 132 | ORB_DECLARE(example_struct_str, orb_example_struct_str); 133 | 134 | struct orb_example_struct_number { 135 | int16_t num_int16; 136 | int32_t num_int32; 137 | int64_t num_int64; 138 | }; 139 | 140 | ORB_DECLARE(example_struct_number, orb_example_struct_number); 141 | ``` 142 | 143 | #### Define uORB topics 144 | 145 | Similar to the declaration, define uORB topics in the source file after including uorb/uorb.h. 146 | 147 | ```C++ 148 | // orb_topic.cc 149 | #include "orb_topic.h" 150 | 151 | ORB_SIMPLE_DEFINE(example_struct_str, orb_example_struct_str); 152 | 153 | ORB_SIMPLE_DEFINE(example_struct_number, orb_example_struct_number); 154 | ``` 155 | 156 | ## Publish uORB topic 157 | 158 | Include the uORB publication header: 159 | 160 | ```C++ 161 | #include "uorb/publication.h" 162 | ``` 163 | 164 | Include the header file of the topic you want to post: 165 | 166 | ```c++ 167 | // Header file generated by topic generation tool 168 | #include "uorb/topics/example_string.h" 169 | 170 | // Or include manually defined topic header files 171 | #include "orb_topic.h" 172 | ``` 173 | 174 | Define a data type for publishing, use `uorb::msg::{topic_name}` to specify the message name (`*.msg` file name or topic name declared with `ORB_DECLARE`): 175 | 176 | ```c++ 177 | uorb::PublicationData pub_example_string; 178 | ``` 179 | 180 | Fill data: 181 | 182 | ```C++ 183 | auto &data = pub_example_string.get(); 184 | 185 | data.timestamp = orb_absolute_time_us(); // orb_absolute_time_us() from "uorb/abs_time.h" 186 | snprintf((char *)data.string, example_string_s::STRING_LENGTH, "%d: %s", i, 187 | "This is a string message."); 188 | ``` 189 | Publish data: 190 | 191 | ```c++ 192 | pub_example_string.Publish(); 193 | ``` 194 | 195 | Please refer to the complete routine: [examples/cpp_pub_sub/cpp_pub_sub.cc](../examples/cpp_pub_sub/cpp_pub_sub.cc) 196 | 197 | ## Subscribe to uORB topic 198 | 199 | Include the uORB subscription header: 200 | 201 | ```c++ 202 | #include "uorb/subscription.h" 203 | ``` 204 | 205 | Define variables of the subscription data type: 206 | 207 | ```c++ 208 | uorb::SubscriptionDatauorb::msg::example_string sub_example_string; 209 | ``` 210 | 211 | Polling for updates: 212 | 213 | ```c++ 214 | if (sub_example_string.Update()) { 215 | // Data processing... 216 | } 217 | ``` 218 | 219 | Or use `orb_poll()`, which is a function similar to `poll()`(Recommend this usage): 220 | 221 | ```c++ 222 | struct orb_pollfd pollfds[] = { 223 | {.fd = sub_example_string.handle(), .events = POLLIN}}; 224 | 225 | if (0 < orb_poll(pollfds, ARRAY_SIZE(pollfds), timeout_ms)) { 226 | if (sub_example_string.Update()) { 227 | // Data processing... 228 | } 229 | } 230 | ``` 231 | 232 | Get updated data and processing: 233 | 234 | ```c++ 235 | auto data = sub_example_string.get(); 236 | printf("timestamp: %" PRIu64 "[us], Receive msg: "%s"\n", data.timestamp, 237 | data.string); 238 | ``` 239 | 240 | Please refer to the complete routine: [examples/cpp_pub_sub/cpp_pub_sub.cc](../examples/cpp_pub_sub/cpp_pub_sub.cc) 241 | 242 | -------------------------------------------------------------------------------- /docs/getting_started_cn.md: -------------------------------------------------------------------------------- 1 | # 添加uORB库到项目中 2 | 3 | 有两种方式可以在你的项目中使用uORB： 4 | 5 | * [使用预构建的uorb二进制库和头文件](#使用uORB的预构建库)。如果你想使用稳定版本的uORB库就使用这个方法。 6 | * [从源代码构建uORB](#从源代码构建uORB)。如果你想调试或者使用最新的修改就使用这个方法。 7 | 8 | ## 使用uORB的预构建库 9 | 10 | 建议您在源目录之外构建uORB，以避免CMake生成的文件出现污染源目录的问题： 11 | 12 | ```shell 13 | mkdir uorb-build 14 | cd uorb-build 15 | cmake -DCMAKE_C_COMPILER=clang \ 16 | -DCMAKE_CXX_COMPILER=clang++ \ 17 | -DCMAKE_INSTALL_PREFIX=./install \ 18 | $HOME/uorb # we're assuming this is where uorb is. 19 | make 20 | make install 21 | ``` 22 | 23 | `./install`目录包含一个lib目录和一个include目录，链接lib目录中的库，然后将include文件夹添加到项目的include路径。 24 | 25 | 或参考工具链目录（`toolchains`）中的`x.toolchain.cmake`文件，为相应平台编写一个cmake工具链文件。 26 | 27 | ## 从源代码构建uORB 28 | 29 | 从源代码构建的最佳方法是将其作为cmake项目的子目录： 30 | 31 | 将源代码添加到您的项目。 32 | 33 | ```shell 34 | git submodule add https://github.com/ShawnFeng0/uorb.git # If you don't use git, just download the source code and unzip it. 35 | ``` 36 | 37 | 在您自己的项目的CMakeLists.txt中链接uORB库。 38 | 39 | ```cmake 40 | # uORB library 41 | add_subdirectory(uorb) 42 | target_link_libraries(${PROJECT_NAME} PRIVATE uorb) # Link the uorb library and add the include path of uorb 43 | ``` 44 | 45 | 这是使用此方法的一个示例（**推荐**）[uorb-examples](https://github.com/ShawnFeng0/uorb-examples.git). 46 | 47 | # 使用uORB 48 | 49 | uORB基于话题而不是数据流进行通信，并且每个话题都有对应的结构体。 50 | 51 | uORB的C接口在uorb / uorb.h中定义。 52 | 53 | ## 创建uORB话题 54 | 55 | uORB话题由以下宏定义，需要传入话题名称和与该话题对应的结构体参数。 56 | 57 | ```C++ 58 | // uorb/uorb.h 59 | // To declare a topic, usually declare a topic in the header file. 60 | #define ORB_DECLARE(_name, _struct) ... 61 | ``` 62 | 63 | ```C++ 64 | // uorb/uorb.h 65 | // Define a topic, usually define a topic in the source file, can only be defined once and need to be compiled in the C++ source file (*.cpp/*.cc) 66 | #define ORB_SIMPLE_DEFINE(_name, _struct) ... 67 | ``` 68 | 69 | ### 使用工具管理uORB话题 70 | 71 | 为了以统一的方式管理和修改话题，我们使用消息生成工具，该工具使用类似于ROS的方式来定义话题。 72 | 73 | 我偷了个懒 ： ）。 我目前不熟悉代码生成工具，所以目前仍使用PX4的代码生成工具（可能会在以后删除），但是用于生成uORB话题的模板文件已被修改。 74 | 75 | #### 添加uORB话题 76 | 77 | 首先，创建一个文件夹来存储uORB话题（原始话题是使用.msg文件定义的，它将生成.h / .cc话题文件）。 然后添加一个话题文件。 78 | 79 | ``` 80 | # msg/example_string.msg 81 | 82 | uint64 timestamp # time since system start (microseconds) 83 | 84 | uint32 STRING_LENGTH = 128 85 | int8[128] string 86 | ``` 87 | 88 | 有关msg文件的格式，请参阅ros和PX4或该项目中的示例。 89 | 90 | 使用此项目的tools / msg目录中的消息生成工具，可以从* .msg文件生成* .h和* .cc话题源文件。 有许多工具参数。 当前建议参考该项目的示例目录，或者独立的uorb示例项目msg / CMakeLists.txt实现消息生成。 91 | 92 | 请注意，msg / CMakeLists.txt中的设置生成工具路径的地址必须正确。 93 | 94 | ```Cmake 95 | # Need to specify the path of generator and message template, they are in the tools/msg directory of this project. 96 | set(tools_root ${CMAKE_CURRENT_SOURCE_DIR}/../../tools/msg) 97 | ``` 98 | 99 | #### 添加uORB话题作为依赖项 100 | 101 | 将以下语句添加到您创建的msg目录级别的CMakeLists.txt中，以使用话题库作为依赖项。 102 | 103 | ```cmake 104 | # Generate orb message 105 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/msg) 106 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/msg) 107 | 108 | # The name of your topic library. It is a library generated after compiling all [topic].cc files. Please refer to the example implementation for details. 109 | link_libraries(uorb_examples_msgs) 110 | ``` 111 | 112 | ### 手动管理uORB话题 113 | 114 | 尽管不建议这样做，但我们也支持直接使用宏手动定义话题，这可能对初学者很有用。 您需要确保在话题头文件中使用`ORB_DECLARE`声明，并在C++源文件中使用`ORB_SIMPLE_DEFINE`定义。 115 | 116 | #### 声明uORB话题 117 | 118 | 只需在话题头文件中包含`uorb/uorb.h`头文件，然后直接声明话题即可。 119 | 120 | ```C++ 121 | // orb_topic.h 122 | #pragma once 123 | 124 | #include "uorb/uorb.h" 125 | 126 | struct orb_example_struct_str { 127 | char str[30]; 128 | int32_t index; 129 | }; 130 | 131 | ORB_DECLARE(example_struct_str, orb_example_struct_str); 132 | 133 | struct orb_example_struct_number { 134 | int16_t num_int16; 135 | int32_t num_int32; 136 | int64_t num_int64; 137 | }; 138 | 139 | ORB_DECLARE(example_struct_number, orb_example_struct_number); 140 | ``` 141 | 142 | #### 定义uORB话题 143 | 144 | 与声明类似，在包含`uorb/uorb.h`之后在源文件中定义uORB话题。 145 | 146 | ```C++ 147 | // orb_topic.cc 148 | #include "orb_topic.h" 149 | 150 | ORB_SIMPLE_DEFINE(example_struct_str, orb_example_struct_str); 151 | 152 | ORB_SIMPLE_DEFINE(example_struct_number, orb_example_struct_number); 153 | ``` 154 | 155 | ## 发布uORB话题 156 | 157 | 包含uORB发布头文件： 158 | 159 | ```C++ 160 | #include "uorb/publication.h" 161 | ``` 162 | 163 | 包含要发布的话题的头文件： 164 | 165 | ```c++ 166 | // Header file generated by topic generation tool 167 | #include "uorb/topics/example_string.h" 168 | 169 | // Or include manually defined topic header files 170 | #include "orb_topic.h" 171 | ``` 172 | 173 | 定义要发布的数据类型，使用`uorb::msg::{topic_name}`指定消息名称（*.msg文件名或用`ORB_DECLARE`声明的话题名）： 174 | 175 | ```c++ 176 | uorb::PublicationData pub_example_string; 177 | ``` 178 | 179 | 填充数据： 180 | 181 | ```C++ 182 | auto &data = pub_example_string.get(); 183 | 184 | data.timestamp = orb_absolute_time_us(); // orb_absolute_time_us() from "uorb/abs_time.h" 185 | snprintf((char *)data.string, example_string_s::STRING_LENGTH, "%d: %s", i, 186 | "This is a string message."); 187 | ``` 188 | 发布数据： 189 | 190 | ```c++ 191 | pub_example_string.Publish(); 192 | ``` 193 | 194 | 请参考完整的例程：[examples/cpp_pub_sub/cpp_pub_sub.cc](../examples/cpp_pub_sub/cpp_pub_sub.cc) 195 | 196 | ## 订阅uORB话题 197 | 198 | 包含uORB订阅头文件: 199 | 200 | ```c++ 201 | #include "uorb/subscription.h" 202 | ``` 203 | 204 | 定义订阅数据类型的变量： 205 | 206 | ```c++ 207 | uorb::SubscriptionDatauorb::msg::example_string sub_example_string; 208 | ``` 209 | 210 | 轮询更新： 211 | 212 | ```c++ 213 | if (sub_example_string.Update()) { 214 | // Data processing... 215 | } 216 | ``` 217 | 218 | 或使用`orb_poll()`，该函数类似于`poll()`（推荐此用法）： 219 | 220 | ```c++ 221 | struct orb_pollfd pollfds[] = { 222 | {.fd = sub_example_string.handle(), .events = POLLIN}}; 223 | 224 | if (0 < orb_poll(pollfds, ARRAY_SIZE(pollfds), timeout_ms)) { 225 | if (sub_example_string.Update()) { 226 | // Data processing... 227 | } 228 | } 229 | ``` 230 | 231 | 获取更新的数据和处理： 232 | 233 | ```c++ 234 | auto data = sub_example_string.get(); 235 | printf("timestamp: %" PRIu64 "[us], Receive msg: "%s"\n", data.timestamp, 236 | data.string); 237 | ``` 238 | 239 | 请参考完整的例程：[examples/cpp_pub_sub/cpp_pub_sub.cc](../examples/cpp_pub_sub/cpp_pub_sub.cc) 240 | 241 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(uorb_examples) 2 | 3 | cmake_minimum_required(VERSION 3.12...4.0) 4 | 5 | # Common header: slog.h 6 | add_library(slog INTERFACE) 7 | target_include_directories(slog INTERFACE common) 8 | 9 | # Generate orb message 10 | add_subdirectory(msg) 11 | 12 | # Examples 13 | add_subdirectory(c_pub_sub) 14 | add_subdirectory(cpp_pub_sub) 15 | add_subdirectory(tcp_topic_listener) 16 | -------------------------------------------------------------------------------- /examples/c_pub_sub/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(uorb_example_c_pub_sub) 2 | 3 | add_executable(${PROJECT_NAME} c_pub_sub.c) 4 | target_link_libraries(${PROJECT_NAME} PRIVATE slog) 5 | target_link_libraries(${PROJECT_NAME} PRIVATE uorb_examples_msgs) 6 | 7 | # pthread 8 | find_package(Threads REQUIRED) 9 | target_link_libraries(${PROJECT_NAME} PRIVATE Threads::Threads) 10 | -------------------------------------------------------------------------------- /examples/c_pub_sub/c_pub_sub.c: -------------------------------------------------------------------------------- 1 | // 2 | // Created by fs on 2020-01-15. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "slog.h" 10 | #include "uorb/topics/example_string.h" 11 | 12 | void *thread_publisher(void *arg) { 13 | (void)arg; 14 | struct example_string_s example_string; 15 | orb_publication_t *pub_example_string = 16 | orb_create_publication(ORB_ID(example_string)); 17 | 18 | for (int i = 0; i < 10; i++) { 19 | snprintf((char *)example_string.str, EXAMPLE_STRING_STRING_LENGTH, "%d: %s", 20 | i, "This is a string message."); 21 | 22 | if (!orb_publish(pub_example_string, &example_string)) { 23 | LOGGER_ERROR("Publish error"); 24 | } 25 | usleep(1 * 1000 * 1000); 26 | } 27 | 28 | orb_destroy_publication(&pub_example_string); 29 | LOGGER_WARN("Publication over."); 30 | 31 | return NULL; 32 | } 33 | 34 | void *thread_subscriber(void *unused) { 35 | (void)unused; 36 | orb_subscription_t *sub_example_string = 37 | orb_create_subscription(ORB_ID(example_string)); 38 | 39 | #ifndef ARRAY_SIZE 40 | #define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) 41 | #endif 42 | 43 | struct orb_pollfd pollfds[] = {{.fd = sub_example_string, .events = POLLIN}}; 44 | int timeout = 2000; 45 | 46 | while (true) { 47 | if (0 < orb_poll(pollfds, ARRAY_SIZE(pollfds), timeout)) { 48 | struct example_string_s example_string; 49 | orb_copy(sub_example_string, &example_string); 50 | LOGGER_INFO("Receive msg: \"%s\"", example_string.str); 51 | } else { 52 | LOGGER_WARN("Got no data within %d milliseconds", timeout); 53 | break; 54 | } 55 | } 56 | 57 | orb_destroy_subscription(&sub_example_string); 58 | 59 | LOGGER_WARN("subscription over"); 60 | return NULL; 61 | } 62 | 63 | int main() { 64 | LOGGER_INFO("uORB version: %s", orb_version()); 65 | 66 | // One publishing thread, three subscription threads 67 | pthread_t pthread_id; 68 | pthread_create(&pthread_id, NULL, thread_publisher, NULL); 69 | pthread_create(&pthread_id, NULL, thread_subscriber, NULL); 70 | pthread_create(&pthread_id, NULL, thread_subscriber, NULL); 71 | pthread_create(&pthread_id, NULL, thread_subscriber, NULL); 72 | 73 | // Wait for all threads to finish 74 | pthread_exit(NULL); 75 | } 76 | -------------------------------------------------------------------------------- /examples/common/slog.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | // Precompiler define to get only filename; 10 | #if !defined(__FILENAME__) 11 | #define __FILENAME__ \ 12 | ({ \ 13 | strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 \ 14 | : strrchr(__FILE__, '\\') ? strrchr(__FILE__, '\\') + 1 \ 15 | : __FILE__; \ 16 | }) 17 | #endif 18 | 19 | #define LOGGER_LEVEL_TRACE "T" 20 | #define LOGGER_LEVEL_DEBUG "D" 21 | #define LOGGER_LEVEL_INFO "I" 22 | #define LOGGER_LEVEL_WARN "W" 23 | #define LOGGER_LEVEL_ERROR "E" 24 | #define LOGGER_LEVEL_FATAL "F" 25 | 26 | #define LOGGER_RESET_COLOR "\x1b[0m" 27 | #define LOGGER_TRACE_COLOR "\x1b[37m" 28 | #define LOGGER_DEBUG_COLOR "\x1b[34m" 29 | #define LOGGER_INFO_COLOR "\x1b[32m" 30 | #define LOGGER_WARN_COLOR "\x1b[33m" 31 | #define LOGGER_ERROR_COLOR "\x1b[31m" 32 | #define LOGGER_FATAL_COLOR "\x1b[35m" 33 | 34 | #define LOGGER_OUT(level, fmt, ...) \ 35 | do { \ 36 | printf("%s/(%s:%d %s) %s" fmt LOGGER_RESET_COLOR "\r\n", \ 37 | LOGGER_LEVEL_##level, __FILENAME__, __LINE__, __FUNCTION__, \ 38 | LOGGER_##level##_COLOR, ##__VA_ARGS__); \ 39 | } while (0) 40 | 41 | #define LOGGER_TRACE(fmt, ...) LOGGER_OUT(TRACE, fmt, ##__VA_ARGS__) 42 | #define LOGGER_DEBUG(fmt, ...) LOGGER_OUT(DEBUG, fmt, ##__VA_ARGS__) 43 | #define LOGGER_INFO(fmt, ...) LOGGER_OUT(INFO, fmt, ##__VA_ARGS__) 44 | #define LOGGER_WARN(fmt, ...) LOGGER_OUT(WARN, fmt, ##__VA_ARGS__) 45 | #define LOGGER_ERROR(fmt, ...) LOGGER_OUT(ERROR, fmt, ##__VA_ARGS__) 46 | #define LOGGER_FATAL(fmt, ...) LOGGER_OUT(FATAL, fmt, ##__VA_ARGS__) 47 | -------------------------------------------------------------------------------- /examples/cpp_pub_sub/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(uorb_example_cpp_pub_sub) 2 | 3 | add_executable(${PROJECT_NAME} cpp_pub_sub.cc) 4 | target_link_libraries(${PROJECT_NAME} PRIVATE slog) 5 | target_link_libraries(${PROJECT_NAME} PRIVATE uorb_examples_msgs) 6 | 7 | # pthread 8 | find_package(Threads REQUIRED) 9 | target_link_libraries(${PROJECT_NAME} PRIVATE Threads::Threads) 10 | -------------------------------------------------------------------------------- /examples/cpp_pub_sub/cpp_pub_sub.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | 5 | #include 6 | #include 7 | 8 | #include "slog.h" 9 | #include "uorb/abs_time.h" 10 | #include "uorb/publication.h" 11 | #include "uorb/publication_multi.h" 12 | #include "uorb/subscription.h" 13 | #include "uorb/subscription_interval.h" 14 | #include "uorb/topics/example_string.h" 15 | 16 | void *thread_publisher(void *unused) { 17 | (void)unused; 18 | uorb::PublicationData pub_example_string; 19 | 20 | for (int i = 0; i < 10; i++) { 21 | auto &data = pub_example_string.get(); 22 | 23 | data.timestamp = orb_absolute_time_us(); 24 | snprintf(reinterpret_cast(data.str), 25 | example_string_s::STRING_LENGTH, "%d: %s", i, 26 | "This is a string message."); 27 | 28 | if (!pub_example_string.Publish()) { 29 | LOGGER_ERROR("Publish error"); 30 | } 31 | 32 | usleep(1 * 1000 * 1000); 33 | } 34 | LOGGER_WARN("Publication over."); 35 | 36 | return nullptr; 37 | } 38 | 39 | void *thread_subscriber(void *unused) { 40 | (void)unused; 41 | uorb::SubscriptionData sub_example_string; 42 | 43 | #ifndef ARRAY_SIZE 44 | #define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) 45 | #endif 46 | 47 | int timeout_ms = 2000; 48 | 49 | struct orb_pollfd poll_fds[] = { 50 | {.fd = sub_example_string.handle(), .events = POLLIN, .revents = 0}}; 51 | 52 | while (true) { 53 | if (0 < orb_poll(poll_fds, ARRAY_SIZE(poll_fds), timeout_ms)) { 54 | if (sub_example_string.Update()) { 55 | auto data = sub_example_string.get(); 56 | LOGGER_INFO("timestamp: %" PRIu64 "[us], Receive msg: \"%s\"", 57 | data.timestamp, data.str); 58 | } 59 | } else { 60 | LOGGER_WARN("Got no data within %d milliseconds", 2000); 61 | break; 62 | } 63 | } 64 | 65 | LOGGER_WARN("subscription over"); 66 | return nullptr; 67 | } 68 | 69 | int main(int, char *[]) { 70 | LOGGER_INFO("uORB version: %s", orb_version()); 71 | 72 | // One publishing thread, three subscription threads 73 | pthread_t pthread_id; 74 | pthread_create(&pthread_id, nullptr, thread_publisher, nullptr); 75 | pthread_detach(pthread_id); 76 | 77 | pthread_create(&pthread_id, nullptr, thread_subscriber, nullptr); 78 | pthread_detach(pthread_id); 79 | 80 | pthread_create(&pthread_id, nullptr, thread_subscriber, nullptr); 81 | pthread_detach(pthread_id); 82 | 83 | pthread_create(&pthread_id, nullptr, thread_subscriber, nullptr); 84 | pthread_detach(pthread_id); 85 | 86 | // Wait for all threads to finish 87 | pthread_exit(nullptr); 88 | } 89 | -------------------------------------------------------------------------------- /examples/msg/.gitignore: -------------------------------------------------------------------------------- 1 | *.[ch]pp 2 | *.cc 3 | *.[ch] 4 | -------------------------------------------------------------------------------- /examples/msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(uorb_examples_msgs LANGUAGES NONE) 2 | cmake_minimum_required(VERSION 3.12...4.0) 3 | 4 | # Support IN_LIST if() operator 5 | cmake_policy(SET CMP0057 NEW) 6 | 7 | find_package (Python3 COMPONENTS Interpreter Development) 8 | # We have a custom error message to tell users how to install python3. 9 | if (NOT Python3_Interpreter_FOUND) 10 | message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n" 11 | " Ubuntu: sudo apt install python3 python3-dev python3-pip\n" 12 | " macOS: brew install python") 13 | endif () 14 | 15 | file(GLOB msg_files 16 | *.msg 17 | ) 18 | 19 | # headers 20 | set(msg_out_path ${CMAKE_CURRENT_SOURCE_DIR}/uorb/topics) 21 | set(msg_source_out_path ${CMAKE_CURRENT_SOURCE_DIR}/uorb/topics_sources) 22 | 23 | set(uorb_headers) 24 | set(uorb_sources ${msg_source_out_path}/uorb_topics.cc) 25 | foreach (msg_file ${msg_files}) 26 | get_filename_component(msg ${msg_file} NAME_WE) 27 | list(APPEND uorb_headers ${msg_out_path}/${msg}.h) 28 | list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cc) 29 | endforeach () 30 | 31 | set(tools_root ${CMAKE_CURRENT_SOURCE_DIR}/../../tools/msg) 32 | 33 | # Generate uORB headers 34 | add_custom_command(OUTPUT ${uorb_headers} 35 | COMMAND ${PYTHON_EXECUTABLE} ${tools_root}/tools/px_generate_uorb_topic_files.py 36 | --headers 37 | -f ${msg_files} 38 | -i ${CMAKE_CURRENT_SOURCE_DIR} 39 | -o ${msg_out_path} 40 | -e ${tools_root}/templates/uorb 41 | -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers 42 | -q 43 | DEPENDS 44 | ${msg_files} 45 | ${tools_root}/templates/uorb/msg.h.em 46 | ${tools_root}/templates/uorb/uorb_topics.h.em 47 | ${tools_root}/tools/px_generate_uorb_topic_files.py 48 | COMMENT "Generating uORB topic headers" 49 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 50 | VERBATIM 51 | ) 52 | 53 | # Generate uORB sources 54 | add_custom_command(OUTPUT ${uorb_sources} 55 | COMMAND ${PYTHON_EXECUTABLE} ${tools_root}/tools/px_generate_uorb_topic_files.py 56 | --sources 57 | -f ${msg_files} 58 | -i ${CMAKE_CURRENT_SOURCE_DIR} 59 | -o ${msg_source_out_path} 60 | -e ${tools_root}/templates/uorb 61 | -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources 62 | -q 63 | ${added_arguments} 64 | DEPENDS 65 | ${msg_files} 66 | ${tools_root}/templates/uorb/msg.cc.em 67 | ${tools_root}/templates/uorb/uorb_topics.cc.em 68 | ${tools_root}/tools/px_generate_uorb_topic_files.py 69 | COMMENT "Generating uORB topic sources" 70 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 71 | VERBATIM 72 | ) 73 | 74 | add_library(${PROJECT_NAME} ${uorb_sources} ${uorb_headers}) 75 | target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 76 | target_link_libraries(${PROJECT_NAME} PUBLIC uorb) 77 | -------------------------------------------------------------------------------- /examples/msg/example_string.msg: -------------------------------------------------------------------------------- 1 | uint64 timestamp # time since system start (microseconds) 2 | 3 | uint32 STRING_LENGTH = 128 4 | int8[128] str 5 | 6 | uint16 ORB_QUEUE_SIZE = 3 7 | -------------------------------------------------------------------------------- /examples/msg/msg_template.msg: -------------------------------------------------------------------------------- 1 | # Timestamp, it is recommended that every message must have 2 | uint64 timestamp # time since system start (microseconds) 3 | 4 | # General data type 5 | char char_value 6 | bool bool_value 7 | int8 s8_value 8 | int16 s16_value 9 | int32 s32_value 10 | int64 s64_value 11 | 12 | uint8 u8_value 13 | uint16 u16_value 14 | uint32 u32_value 15 | uint64 u64_value 16 | 17 | float32 f32_value 18 | float64 f64_value 19 | 20 | # Array 21 | uint32[4] uint32_array # Can be of any type 22 | int8[5] str 23 | 24 | # Constant 25 | # Will generate a macro (For C) and a constant in the structure (For C++) 26 | uint32 u32_constant = 10 # Can be of any type 27 | -------------------------------------------------------------------------------- /examples/msg/sensor_accel.msg: -------------------------------------------------------------------------------- 1 | uint64 timestamp # time since system start (microseconds) 2 | uint64 timestamp_sample 3 | 4 | uint32 device_id # unique device ID for the sensor that does not change between power cycles 5 | 6 | float32 x # acceleration in the NED X board axis in m/s^2 7 | float32 y # acceleration in the NED Y board axis in m/s^2 8 | float32 z # acceleration in the NED Z board axis in m/s^2 9 | 10 | float32 temperature # temperature in degrees celsius 11 | -------------------------------------------------------------------------------- /examples/msg/sensor_gyro.msg: -------------------------------------------------------------------------------- 1 | uint64 timestamp # time since system start (microseconds) 2 | uint64 timestamp_sample 3 | 4 | uint32 device_id # unique device ID for the sensor that does not change between power cycles 5 | 6 | float32 x # angular velocity in the NED X board axis in rad/s 7 | float32 y # angular velocity in the NED Y board axis in rad/s 8 | float32 z # angular velocity in the NED Z board axis in rad/s 9 | 10 | float32 temperature # temperature in degrees celsius 11 | -------------------------------------------------------------------------------- /examples/tcp_topic_listener/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(uorb_example_tcp_topic_listener) 2 | 3 | add_executable(${PROJECT_NAME} tcp_topic_listener_test.cc) 4 | target_link_libraries(${PROJECT_NAME} PRIVATE slog) 5 | target_link_libraries(${PROJECT_NAME} PRIVATE uorb_examples_msgs) 6 | target_link_libraries(${PROJECT_NAME} PRIVATE uorb_tcp_topic_listener_lib) 7 | -------------------------------------------------------------------------------- /examples/tcp_topic_listener/README.md: -------------------------------------------------------------------------------- 1 | # uorb topic TCP listener 2 | 3 | uorb listener is a tcp server for uorb topic monitoring, which can monitor uorb topic data, uorb topic status, etc. 4 | 5 | This is an example of uorb tcp listener. When the routine starts, we can use the tcp client to connect and send 6 | commands. 7 | 8 | The following description assumes that this example has been run and executed locally. 9 | 10 | ## View help 11 | 12 | Support the help command, view all supported commands and a brief introduction. 13 | 14 | ### Command 15 | 16 | ```shell 17 | echo "help" | nc 127.0.0.1 10924 18 | ``` 19 | 20 | ### Example 21 | 22 | ```text 23 | ~ > echo "help" | nc -v 127.0.0.1 10924 24 | localhost [127.0.0.1] 10924 open 25 | Command list: 26 | help: Print command list 27 | listener: topic listener, example: listener topic_name 28 | status: Print uorb status 29 | version: Print uorb version 30 | ``` 31 | 32 | ## View the status of uorb 33 | 34 | View the status of all topics in uorb, including the topic name of each topic, queue size, number of 35 | publishers/subscribers, and the latest data publication index. 36 | 37 | ### Command 38 | 39 | ```shell 40 | echo "status" | nc 127.0.0.1 10924 41 | ``` 42 | 43 | After the output, the tcp connection will be automatically disconnected. 44 | 45 | ### Example 46 | 47 | ```text 48 | ~ > echo "status" | nc -v 127.0.0.1 10924 49 | localhost [127.0.0.1] 10924 open 50 | topic instance queue sub pub index 51 | example_string 0 4 3+ 3+ 109 52 | sensor_accel 0 1 3 4 29888 53 | sensor_gyro 0 1 3 0 0 54 | ``` 55 | 56 | ## Monitor real-time data on uorb topics 57 | 58 | With the help of topic metadata, we can parse out the topic's binary data. The maximum monitoring frequency of topics is 59 | 10hz. 60 | 61 | ### Command 62 | 63 | ```shell 64 | echo "listener topic_name" | nc 127.0.0.1 10924 65 | ``` 66 | 67 | The data will be printed to the terminal continuously, use `Ctrl+C` to terminate. 68 | 69 | If you forget the topic name, you can first use the status command to view all topics and find the topic name you want. 70 | 71 | ### Example 72 | 73 | ```text 74 | ~ > echo "listener sensor_accel" | nc -v 127.0.0.1 10924 75 | localhost [127.0.0.1] 10924 open 76 | uint64_t timestamp = 604868299279 77 | uint64_t timestamp_sample = 604868299279 78 | uint32_t device_id = 10 79 | float x = 998831.000000 80 | float y = 1997662.000000 81 | float z = 2996493.000000 82 | float temperature = 3995324.000000 83 | uint8_t[4] _padding0 = [0, 0, 0, 0] 84 | ... 85 | ``` 86 | -------------------------------------------------------------------------------- /examples/tcp_topic_listener/tcp_topic_listener_test.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "slog.h" 12 | #include "uorb/publication.h" 13 | #include "uorb/publication_multi.h" 14 | #include "uorb/subscription.h" 15 | #include "uorb/subscription_interval.h" 16 | #include "uorb/topics/example_string.h" 17 | #include "uorb/topics/msg_template.h" 18 | #include "uorb/topics/sensor_accel.h" 19 | #include "uorb/topics/sensor_gyro.h" 20 | #include "uorb/topics/uorb_topics.h" 21 | #include "uorb_tcp_listener.h" 22 | 23 | template 24 | [[noreturn]] static void thread_publisher() { 25 | uorb::PublicationData publication_data; 26 | 27 | while (true) { 28 | auto &data = publication_data.get(); 29 | 30 | data.timestamp = orb_absolute_time_us(); 31 | 32 | if (!publication_data.Publish()) { 33 | LOGGER_ERROR("Publish error"); 34 | } 35 | 36 | usleep(1 * 1000 * 1000); 37 | } 38 | LOGGER_WARN("Publication over."); 39 | } 40 | 41 | [[noreturn]] static void thread_publisher_sensor_accel() { 42 | uorb::PublicationData publication_data; 43 | 44 | while (true) { 45 | auto &data = publication_data.get(); 46 | 47 | data.timestamp = orb_absolute_time_us(); 48 | data.timestamp_sample = data.timestamp; 49 | data.device_id = 10; 50 | data.x += 1; 51 | data.y += 2; 52 | data.z += 3; 53 | data.temperature += 4; 54 | 55 | if (!publication_data.Publish()) { 56 | LOGGER_ERROR("Publish error"); 57 | } 58 | 59 | usleep(1 * 1000); 60 | } 61 | LOGGER_WARN("Publication over."); 62 | } 63 | 64 | template 65 | [[noreturn]] static void thread_subscriber() { 66 | uorb::SubscriptionData subscription_data; 67 | 68 | #ifndef ARRAY_SIZE 69 | #define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) 70 | #endif 71 | 72 | int timeout_ms = 2000; 73 | 74 | struct orb_pollfd poll_fds[] = { 75 | {.fd = subscription_data.handle(), .events = POLLIN, .revents = 0}}; 76 | 77 | while (true) { 78 | if (0 < orb_poll(poll_fds, ARRAY_SIZE(poll_fds), timeout_ms)) { 79 | if (subscription_data.Update()) { 80 | // auto data = sub_example_string.get(); 81 | // LOGGER_INFO("timestamp: %" PRIu64 "[us]", data.timestamp); 82 | } 83 | } 84 | } 85 | } 86 | 87 | int main(int, char *[]) { 88 | LOGGER_INFO("uORB version: %s", orb_version()); 89 | 90 | std::thread{thread_publisher_sensor_accel}.detach(); 91 | 92 | for (int i = 0; i < 3; ++i) 93 | std::thread{thread_publisher}.detach(); 94 | 95 | for (int i = 0; i < 3; ++i) 96 | std::thread{thread_publisher}.detach(); 97 | for (int i = 0; i < 3; ++i) 98 | std::thread{thread_subscriber}.detach(); 99 | 100 | for (int i = 0; i < 3; ++i) 101 | std::thread{thread_subscriber}.detach(); 102 | 103 | for (int i = 0; i < 3; ++i) 104 | std::thread{thread_subscriber}.detach(); 105 | 106 | example_string_s example{}; 107 | example.timestamp = orb_absolute_time_us(); 108 | orb_publish_anonymous(&uorb::msg::example_string, &example); 109 | orb_copy_anonymous(&uorb::msg::example_string, &example); 110 | 111 | orb_tcp_listener_init(orb_get_topics, 10924); 112 | 113 | // Wait for all threads to finish 114 | pthread_exit(nullptr); 115 | } 116 | -------------------------------------------------------------------------------- /examples/tcp_topic_listener/uorb_tcp_client.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # stty -echo -icanon && nc -v localhost 10924 4 | 5 | echo "$*" | nc -v localhost 10924 6 | -------------------------------------------------------------------------------- /include/uorb/abs_time.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | #ifdef __cplusplus 10 | extern "C" { 11 | #endif 12 | 13 | /** 14 | * Absolute time, in microsecond units. 15 | * 16 | * Absolute time is measured from some arbitrary epoch shortly after 17 | * system startup. It should never wrap or go backwards. 18 | */ 19 | typedef uint64_t orb_abstime_us; 20 | 21 | /** 22 | * Get absolute time in [us] (does not wrap). 23 | */ 24 | static inline orb_abstime_us orb_absolute_time_us() { 25 | struct timespec ts = {}; 26 | orb_abstime_us result; 27 | 28 | clock_gettime(CLOCK_MONOTONIC, &ts); 29 | 30 | result = (orb_abstime_us)(ts.tv_sec) * 1000000; 31 | result += ts.tv_nsec / 1000; 32 | 33 | return result; 34 | } 35 | 36 | /** 37 | * Compute the delta between a timestamp taken in the past 38 | * and now. 39 | */ 40 | static inline orb_abstime_us orb_elapsed_time_us(const orb_abstime_us then) { 41 | return orb_absolute_time_us() - then; 42 | } 43 | 44 | #ifdef __cplusplus 45 | } 46 | #endif 47 | 48 | #ifdef __cplusplus 49 | 50 | namespace uorb { 51 | namespace time_literals { 52 | 53 | // User-defined integer literals for different time units. 54 | // The base unit is orb_abstime_us in microseconds 55 | 56 | // NOLINTNEXTLINE 57 | constexpr orb_abstime_us operator"" _s(unsigned long long time) { 58 | return orb_abstime_us(time * 1000000ULL); 59 | } 60 | 61 | // NOLINTNEXTLINE 62 | constexpr orb_abstime_us operator"" _ms(unsigned long long time) { 63 | return orb_abstime_us(time * 1000ULL); 64 | } 65 | 66 | // NOLINTNEXTLINE 67 | constexpr orb_abstime_us operator"" _us(unsigned long long time) { 68 | return orb_abstime_us(time); 69 | } 70 | 71 | } // namespace time_literals 72 | } // namespace uorb 73 | 74 | #endif /* __cplusplus */ 75 | -------------------------------------------------------------------------------- /include/uorb/internal/noncopyable.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 2021/9/9. 3 | // 4 | 5 | #pragma once 6 | 7 | namespace uorb { 8 | namespace internal { 9 | 10 | class Noncopyable { 11 | public: 12 | Noncopyable() = default; 13 | ~Noncopyable() = default; 14 | 15 | Noncopyable(const Noncopyable&) = delete; 16 | Noncopyable& operator=(const Noncopyable&) = delete; 17 | }; 18 | 19 | } // namespace internal 20 | } // namespace uorb 21 | -------------------------------------------------------------------------------- /include/uorb/publication.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace uorb { 7 | 8 | /** 9 | * uORB publication wrapper class 10 | */ 11 | template 12 | class Publication : internal::Noncopyable { 13 | using Type = typename msg::TypeMap::type; 14 | 15 | public: 16 | Publication() noexcept = default; 17 | ~Publication() { handle_ &&orb_destroy_publication(&handle_); } 18 | 19 | /** 20 | * Publish the struct 21 | * @param data The uORB message struct we are updating. 22 | */ 23 | bool Publish(const Type &data) { 24 | if (!handle_) { 25 | handle_ = orb_create_publication(&meta); 26 | } 27 | 28 | return handle_ && orb_publish(handle_, &data); 29 | } 30 | 31 | private: 32 | orb_publication_t *handle_{nullptr}; 33 | }; 34 | 35 | /** 36 | * The publication class with data embedded. 37 | */ 38 | template 39 | class PublicationData : public Publication { 40 | using Type = typename msg::TypeMap::type; 41 | 42 | public: 43 | PublicationData() noexcept = default; 44 | 45 | Type &get() { return data_; } 46 | auto set(const Type &data) -> decltype(*this) { 47 | data_ = data; 48 | return *this; 49 | } 50 | 51 | // Publishes the embedded struct. 52 | bool Publish() { return Publication::Publish(data_); } 53 | 54 | private: 55 | Type data_{}; 56 | }; 57 | 58 | } // namespace uorb 59 | -------------------------------------------------------------------------------- /include/uorb/publication_multi.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace uorb { 7 | 8 | /** 9 | * uORB publication wrapper class 10 | */ 11 | template 12 | class PublicationMulti : internal::Noncopyable { 13 | using Type = typename msg::TypeMap::type; 14 | 15 | public: 16 | PublicationMulti() noexcept = default; 17 | ~PublicationMulti() { handle_ &&orb_destroy_publication(&handle_); } 18 | 19 | /** 20 | * Publish the struct 21 | * @param data The uORB message struct we are updating. 22 | */ 23 | bool Publish(const Type &data) { 24 | if (!handle_) { 25 | unsigned instance; 26 | handle_ = orb_create_publication_multi(&meta, &instance); 27 | } 28 | 29 | return handle_ && orb_publish(handle_, &data); 30 | } 31 | 32 | private: 33 | orb_publication_t *handle_{nullptr}; 34 | }; 35 | 36 | /** 37 | * The publication class with data embedded. 38 | */ 39 | template 40 | class PublicationMultiData : public PublicationMulti { 41 | using Type = typename msg::TypeMap::type; 42 | 43 | public: 44 | PublicationMultiData() noexcept = default; 45 | 46 | Type &get() { return data_; } 47 | auto set(const Type &data) -> decltype(*this) { 48 | data_ = data; 49 | return *this; 50 | } 51 | 52 | // Publishes the embedded struct. 53 | bool Publish() { return PublicationMulti::Publish(data_); } 54 | 55 | private: 56 | Type data_{}; 57 | }; 58 | 59 | } // namespace uorb 60 | -------------------------------------------------------------------------------- /include/uorb/subscription.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace uorb { 8 | 9 | template 10 | class Subscription : internal::Noncopyable { 11 | using Type = typename msg::TypeMap::type; 12 | 13 | protected: 14 | const uint8_t instance_{0}; 15 | orb_subscription_t *handle_{nullptr}; 16 | 17 | /** 18 | * Check if there is a new update. 19 | */ 20 | virtual bool Updated() { return Subscribed() && orb_check_update(handle_); } 21 | 22 | public: 23 | /** 24 | * Constructor 25 | * 26 | * @param meta The uORB metadata (usually from the ORB_ID() macro) for the 27 | * topic. 28 | * @param instance The instance for multi sub. 29 | */ 30 | explicit Subscription(uint8_t instance = 0) noexcept : instance_(instance) {} 31 | 32 | ~Subscription() { handle_ &&orb_destroy_subscription(&handle_); } 33 | 34 | bool Subscribed() { 35 | // check if already subscribed 36 | if (handle_) { 37 | return true; 38 | } 39 | return (handle_ = orb_create_subscription_multi(&meta, instance_)); 40 | } 41 | 42 | decltype(handle_) handle() { return Subscribed() ? handle_ : nullptr; } 43 | 44 | /** 45 | * Update the struct 46 | * @param data The uORB message struct we are updating. 47 | */ 48 | virtual bool Update(Type *dst) { return Updated() && Copy(dst); } 49 | 50 | /** 51 | * Copy the struct 52 | * @param data The uORB message struct we are updating. 53 | */ 54 | virtual bool Copy(Type *dst) { 55 | return Subscribed() && orb_copy(handle_, dst); 56 | } 57 | }; 58 | 59 | // Subscription wrapper class with data 60 | template 61 | class SubscriptionData : public Subscription { 62 | using Type = typename msg::TypeMap::type; 63 | 64 | public: 65 | explicit SubscriptionData(uint8_t instance = 0) noexcept 66 | : Subscription(instance) {} 67 | 68 | ~SubscriptionData() = default; 69 | 70 | using Subscription::Update; 71 | 72 | // update the embedded struct. 73 | bool Update() { return Subscription::Update(&data_); } 74 | 75 | const Type &get() const { return data_; } 76 | 77 | private: 78 | Type data_{}; 79 | }; 80 | 81 | } // namespace uorb 82 | -------------------------------------------------------------------------------- /include/uorb/subscription_interval.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace uorb { 8 | 9 | template 10 | class SubscriptionInterval : public Subscription { 11 | using Type = typename msg::TypeMap::type; 12 | 13 | private: 14 | template 15 | constexpr Tp constrain(Tp val, Tp min_val, Tp max_val) const { 16 | return (val < min_val) ? min_val : ((val > max_val) ? max_val : val); 17 | } 18 | 19 | public: 20 | /** 21 | * Constructor 22 | * 23 | * @param meta The uORB metadata (usually from the ORB_ID() macro) for the 24 | * topic. 25 | * @param interval The requested maximum update interval in microseconds. 26 | * @param instance The instance for multi sub. 27 | */ 28 | explicit SubscriptionInterval(uint32_t interval_us = 0, 29 | uint8_t instance = 0) noexcept 30 | : Subscription(instance), interval_us_(interval_us) {} 31 | 32 | ~SubscriptionInterval() = default; 33 | 34 | /** 35 | * Check if there is a new update. 36 | * */ 37 | bool Updated() override { 38 | return Subscription::Updated() && 39 | (orb_elapsed_time_us(last_update_) >= interval_us_); 40 | } 41 | 42 | /** 43 | * Copy the struct if updated. 44 | * @param dst The destination pointer where the struct will be copied. 45 | * @return true only if topic was updated and copied successfully. 46 | */ 47 | bool Update(Type *dst) override { 48 | if (Updated()) { 49 | return Copy(dst); 50 | } 51 | 52 | return false; 53 | } 54 | 55 | /** 56 | * Copy the struct 57 | * @param dst The destination pointer where the struct will be copied. 58 | * @return true only if topic was copied successfully. 59 | */ 60 | bool Copy(Type *dst) override { 61 | if (Subscription::Copy(dst)) { 62 | const orb_abstime_us now = orb_absolute_time_us(); 63 | // shift last update time forward, but don't let it get further behind 64 | // than the interval 65 | last_update_ = 66 | constrain(last_update_ + interval_us_, now - interval_us_, now); 67 | return true; 68 | } 69 | 70 | return false; 71 | } 72 | 73 | uint32_t interval_us() const { return interval_us_; } 74 | uint32_t interval_ms() const { return interval_us_ / 1000; } 75 | void set_interval_us(uint32_t interval) { interval_us_ = interval; } 76 | void set_interval_ms(uint32_t interval) { interval_us_ = interval * 1000; } 77 | 78 | protected: 79 | orb_abstime_us last_update_{0}; // last update in microseconds 80 | uint32_t interval_us_{0}; // maximum update interval in microseconds 81 | }; 82 | 83 | } // namespace uorb 84 | -------------------------------------------------------------------------------- /src/base/atomic.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2019 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file orb_atomic.h 36 | * 37 | * Provides atomic integers and counters. Each method is executed atomically and 38 | * thus can be used to prevent data races and add memory synchronization between 39 | * threads. 40 | * 41 | * In addition to the atomicity, each method serves as a memory barrier 42 | * (sequential consistent ordering). This means all operations that happen 43 | * before and could potentially have visible side-effects in other threads will 44 | * happen before the method is executed. 45 | * 46 | * The implementation uses the built-in methods from GCC (supported by Clang as 47 | * well). 48 | * @see https://gcc.gnu.org/onlinedocs/gcc/_005f_005fatomic-Builtins.html. 49 | * 50 | * @note: on ARM, the instructions LDREX and STREX might be emitted. To ensure 51 | * correct behavior, the exclusive monitor needs to be cleared on a task switch 52 | * (via CLREX). This happens automatically e.g. on ARMv7-M as part of an 53 | * exception entry or exit sequence. 54 | */ 55 | 56 | #pragma once 57 | 58 | #ifdef __cplusplus 59 | 60 | #include 61 | #include 62 | 63 | /* 64 | * Clang and recent GCC both provide predefined macros for the memory 65 | * orderings. If we are using a compiler that doesn't define them, use the 66 | * clang values - these will be ignored in the fallback path. 67 | */ 68 | 69 | #ifndef __ATOMIC_RELAXED 70 | #define __ATOMIC_RELAXED 0 71 | #endif 72 | #ifndef __ATOMIC_CONSUME 73 | #define __ATOMIC_CONSUME 1 74 | #endif 75 | #ifndef __ATOMIC_ACQUIRE 76 | #define __ATOMIC_ACQUIRE 2 77 | #endif 78 | #ifndef __ATOMIC_RELEASE 79 | #define __ATOMIC_RELEASE 3 80 | #endif 81 | #ifndef __ATOMIC_ACQ_REL 82 | #define __ATOMIC_ACQ_REL 4 83 | #endif 84 | #ifndef __ATOMIC_SEQ_CST 85 | #define __ATOMIC_SEQ_CST 5 86 | #endif 87 | 88 | namespace uorb { 89 | namespace base { 90 | 91 | template 92 | class atomic { 93 | public: 94 | #ifdef __PX4_NUTTX 95 | // Ensure that all operations are lock-free, so that 'atomic' can be used from 96 | // IRQ handlers. This might not be required everywhere though. 97 | static_assert(__atomic_always_lock_free(sizeof(T), 0), 98 | "atomic is not lock-free for the given type T"); 99 | #endif 100 | 101 | atomic() = default; 102 | explicit atomic(T value) : _value(value) {} 103 | 104 | /** 105 | * Atomically read the current value 106 | */ 107 | inline T load() const { 108 | #ifdef __PX4_QURT 109 | return _value; 110 | #else 111 | return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); 112 | #endif 113 | } 114 | 115 | /** 116 | * Atomically store a value 117 | */ 118 | inline void store(T value) { 119 | #ifdef __PX4_QURT 120 | _value = value; 121 | #else 122 | __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); 123 | #endif 124 | } 125 | 126 | /** 127 | * Atomically add a number and return the previous value. 128 | * @return value prior to the addition 129 | */ 130 | inline T fetch_add(T num) { 131 | return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); 132 | } 133 | 134 | /** 135 | * Atomically substract a number and return the previous value. 136 | * @return value prior to the substraction 137 | */ 138 | inline T fetch_sub(T num) { 139 | return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); 140 | } 141 | 142 | /** 143 | * Atomic AND with a number 144 | * @return value prior to the operation 145 | */ 146 | inline T fetch_and(T num) { 147 | return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); 148 | } 149 | 150 | /** 151 | * Atomic XOR with a number 152 | * @return value prior to the operation 153 | */ 154 | inline T fetch_xor(T num) { 155 | return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); 156 | } 157 | 158 | /** 159 | * Atomic OR with a number 160 | * @return value prior to the operation 161 | */ 162 | inline T fetch_or(T num) { 163 | return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); 164 | } 165 | 166 | /** 167 | * Atomic NAND (~(_value & num)) with a number 168 | * @return value prior to the operation 169 | */ 170 | inline T fetch_nand(T num) { 171 | return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); 172 | } 173 | 174 | /** 175 | * Atomic compare and exchange operation. 176 | * This compares the contents of _value with the contents of *expected. If 177 | * equal, the operation is a read-modify-write operation that writes desired 178 | * into _value. If they are not equal, the operation is a read and the current 179 | * contents of _value are written into *expected. 180 | * @return If desired is written into _value then true is returned 181 | */ 182 | inline bool compare_exchange(T *expected, T num) { 183 | return __atomic_compare_exchange(&_value, expected, num, false, 184 | __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); 185 | } 186 | 187 | private: 188 | #ifdef __PX4_QURT 189 | // It seems that __atomic_store and __atomic_load are not supported on Qurt, 190 | // so the best that we can do is to use volatile. 191 | volatile T _value{}; 192 | #else 193 | T _value{}; 194 | #endif 195 | }; 196 | 197 | using atomic_int = atomic; 198 | using atomic_int32_t = atomic; 199 | using atomic_bool = atomic; 200 | 201 | } // namespace base 202 | } // namespace uorb 203 | 204 | #endif /* __cplusplus */ 205 | -------------------------------------------------------------------------------- /src/base/condition_variable.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "base/mutex.h" 11 | #include "uorb/internal/noncopyable.h" 12 | 13 | namespace uorb { 14 | namespace base { 15 | 16 | class ConditionVariableTest; 17 | 18 | class ConditionVariable : public internal::Noncopyable { 19 | public: 20 | ConditionVariable(const ConditionVariable &) = delete; 21 | ConditionVariable &operator=(const ConditionVariable &) = delete; 22 | 23 | ConditionVariable() noexcept { 24 | #ifdef __APPLE__ 25 | pthread_cond_init(&cond_, nullptr); 26 | #else 27 | pthread_condattr_t attr; 28 | pthread_condattr_init(&attr); 29 | pthread_condattr_setclock(&attr, kWhichClock); 30 | pthread_cond_init(&cond_, &attr); 31 | #endif 32 | } 33 | 34 | ~ConditionVariable() noexcept { 35 | // https://chromium.googlesource.com/v8/v8/+/refs/tags/11.5.129/src/base/platform/condition-variable.cc#42 36 | #ifdef __APPLE__ 37 | // This hack is necessary to avoid a fatal pthreads subsystem bug in the 38 | // Darwin kernel. http://crbug.com/517681. 39 | { 40 | Mutex lock; 41 | LockGuard l(lock); 42 | struct timespec ts {}; 43 | ts.tv_sec = 0; 44 | ts.tv_nsec = 1; 45 | pthread_cond_timedwait_relative_np(&cond_, lock.native_handle(), &ts); 46 | } 47 | #endif 48 | pthread_cond_destroy(&cond_); 49 | } 50 | 51 | void notify_one() noexcept { pthread_cond_signal(&cond_); } 52 | 53 | void notify_all() noexcept { pthread_cond_broadcast(&cond_); } 54 | 55 | void wait(Mutex &lock) noexcept { // NOLINT 56 | pthread_cond_wait(&cond_, lock.native_handle()); 57 | } 58 | 59 | template 60 | void wait(Mutex &lock, Predicate p) { // NOLINT 61 | while (!p()) wait(lock); 62 | } 63 | 64 | // Return true if successful 65 | bool wait_for(Mutex &lock, uint32_t time_ms) { // NOLINT 66 | #ifdef __APPLE__ 67 | struct timespec rel_ts = {.tv_sec = time_ms / 1000, 68 | .tv_nsec = (time_ms % 1000) * 1000000}; 69 | // OS X do support waiting on a condition variable with a relative timeout. 70 | auto ret = pthread_cond_timedwait_relative_np(&cond_, lock.native_handle(), 71 | &rel_ts) == 0; 72 | return ret; 73 | #else 74 | struct timespec until_time = timespec_get_after(get_now_time(), time_ms); 75 | auto ret = 76 | pthread_cond_timedwait(&cond_, lock.native_handle(), &until_time) == 0; 77 | return ret; 78 | #endif 79 | } 80 | 81 | // Return true if successful 82 | template 83 | bool wait_for(Mutex &lock, uint32_t time_ms, Predicate p) { // NOLINT 84 | // Not returned until timeout or other error 85 | while (!p()) 86 | if (!wait_for(lock, time_ms)) return p(); 87 | return true; 88 | } 89 | 90 | pthread_cond_t *native_handle() { return &cond_; } 91 | 92 | private: 93 | friend class ConditionVariableTest; 94 | 95 | static inline struct timespec get_now_time() { 96 | struct timespec result {}; 97 | clock_gettime(kWhichClock, &result); 98 | return result; 99 | } 100 | static inline struct timespec timespec_get_after(const struct timespec &now, 101 | uint32_t time_ms) { 102 | static const auto kNSecPerS = 1000 * 1000 * 1000; 103 | 104 | struct timespec result = now; 105 | 106 | result.tv_sec += time_ms / 1000; 107 | result.tv_nsec += (time_ms % 1000) * 1000 * 1000; 108 | 109 | if (result.tv_nsec >= kNSecPerS) { 110 | result.tv_sec += result.tv_nsec / kNSecPerS; 111 | result.tv_nsec %= kNSecPerS; 112 | } 113 | 114 | return result; 115 | } 116 | 117 | pthread_cond_t cond_{}; 118 | 119 | // The C++ specification defines std::condition_variable::wait_for in terms of 120 | // std::chrono::steady_clock, which is closest to CLOCK_MONOTONIC. 121 | static const clockid_t kWhichClock = CLOCK_MONOTONIC; 122 | }; 123 | 124 | class SimpleSemaphore { 125 | public: 126 | explicit SimpleSemaphore(unsigned int count = 0) : count_(count) {} 127 | SimpleSemaphore(const SimpleSemaphore &) = delete; 128 | SimpleSemaphore &operator=(const SimpleSemaphore &) = delete; 129 | 130 | // increments the internal counter and unblocks acquirers 131 | void release() { 132 | LockGuard lock(mutex_); 133 | ++count_; 134 | // The previous semaphore was 0, and there may be waiting tasks 135 | if (count_ == 1) condition_.notify_one(); 136 | } 137 | 138 | // decrements the internal counter or blocks until it can 139 | void acquire() { 140 | LockGuard lock(mutex_); 141 | condition_.wait(mutex_, [&] { return count_ > 0; }); 142 | --count_; 143 | } 144 | 145 | // tries to decrement the internal counter without blocking 146 | bool try_acquire() { 147 | LockGuard lock(mutex_); 148 | if (count_) { 149 | --count_; 150 | return true; 151 | } 152 | return false; 153 | } 154 | 155 | // tries to decrement the internal counter, blocking for up to a duration time 156 | bool try_acquire_for(uint32_t time_ms) { 157 | LockGuard lock(mutex_); 158 | bool finished = 159 | condition_.wait_for(mutex_, time_ms, [&] { return count_ > 0; }); 160 | if (finished) --count_; 161 | return finished; 162 | } 163 | 164 | unsigned int get_value() { 165 | LockGuard lock(mutex_); 166 | return count_; 167 | } 168 | 169 | private: 170 | Mutex mutex_; 171 | ConditionVariable condition_; 172 | unsigned int count_; 173 | }; 174 | 175 | } // namespace base 176 | } // namespace uorb 177 | -------------------------------------------------------------------------------- /src/base/condition_variable_test.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 23-5-13. 3 | // 4 | #include "condition_variable.h" 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include "uorb/abs_time.h" 12 | 13 | #define DEBUG_MARK(mark) \ 14 | printf("%s:%d %" PRIu64 ".%03" PRIu64 "ms " #mark "\r\n", __FILE__, \ 15 | __LINE__, orb_absolute_time_us() / 1000000, \ 16 | (orb_absolute_time_us() / 1000) % 1000) 17 | 18 | class Timer { 19 | public: 20 | Timer() { start_ = std::chrono::high_resolution_clock::now(); } 21 | ~Timer() = default; 22 | 23 | template 24 | uint64_t elapsed() { 25 | auto now = std::chrono::high_resolution_clock::now(); 26 | return std::chrono::duration_cast(now - start_).count(); 27 | } 28 | uint64_t elapsed_us() { return elapsed(); } 29 | uint64_t elapsed_ms() { return elapsed(); } 30 | 31 | private: 32 | std::chrono::time_point start_; 33 | }; 34 | 35 | class ThreadBarrier { 36 | public: 37 | explicit ThreadBarrier(int num_threads) : num_threads_(num_threads) {} 38 | ThreadBarrier(const ThreadBarrier &) = delete; 39 | ThreadBarrier &operator=(const ThreadBarrier &) = delete; 40 | 41 | void wait() { 42 | uorb::base::LockGuard lock(mutex_); 43 | if (--num_threads_ == 0) { 44 | cv_.notify_all(); 45 | } else { 46 | cv_.wait(mutex_, [&]() { return num_threads_ == 0; }); 47 | } 48 | } 49 | 50 | private: 51 | uorb::base::Mutex mutex_; 52 | uorb::base::ConditionVariable cv_; 53 | int num_threads_; 54 | }; 55 | 56 | namespace uorb { 57 | namespace base { 58 | class ConditionVariableTest : public ConditionVariable { 59 | public: 60 | using ConditionVariable::get_now_time; 61 | using ConditionVariable::timespec_get_after; 62 | }; 63 | } // namespace base 64 | } // namespace uorb 65 | 66 | TEST(ConditionVariableTest, get_time) { 67 | for (int ms = 0; ms < 100000; ms++) { 68 | using namespace uorb::base; 69 | auto now = ConditionVariableTest::get_now_time(); 70 | auto now_us = (uint64_t)now.tv_sec * 1000000 + now.tv_nsec / 1000; 71 | auto after = ConditionVariableTest::timespec_get_after(now, ms); 72 | auto after_us = (uint64_t)after.tv_sec * 1000000 + after.tv_nsec / 1000; 73 | EXPECT_EQ(after_us, now_us + ms * 1000); 74 | } 75 | } 76 | 77 | bool wait_for_case(int set_timeout_ms, int wait_timeout_ms, 78 | uint64_t *actual_waiting_time) { 79 | using namespace uorb::base; 80 | auto cv = std::make_shared(); 81 | auto mutex = std::make_shared(); 82 | auto barrier = std::make_shared(2); 83 | auto flag = std::make_shared(false); 84 | std::thread t([=]() { 85 | barrier->wait(); 86 | std::this_thread::sleep_for(std::chrono::milliseconds(set_timeout_ms)); 87 | { 88 | LockGuard<> lg(*mutex); 89 | *flag = true; 90 | } 91 | cv->notify_all(); 92 | }); 93 | barrier->wait(); 94 | Timer timer; 95 | bool ret; 96 | { 97 | LockGuard<> lg(*mutex); 98 | ret = cv->wait_for(*mutex, wait_timeout_ms, [&]() { return *flag; }); 99 | } 100 | if (actual_waiting_time) *actual_waiting_time = timer.elapsed_ms(); 101 | t.join(); 102 | return ret; 103 | } 104 | 105 | TEST(ConditionVariableTest, wait_for) { 106 | { 107 | uint64_t actual_waiting_time; 108 | EXPECT_TRUE(wait_for_case(10, 50, &actual_waiting_time)); 109 | EXPECT_GE(actual_waiting_time, 10); 110 | EXPECT_LE(actual_waiting_time, 50); 111 | } 112 | { 113 | uint64_t actual_waiting_time; 114 | EXPECT_TRUE(wait_for_case(50, 200, &actual_waiting_time)); 115 | EXPECT_GE(actual_waiting_time, 50); 116 | EXPECT_LE(actual_waiting_time, 200); 117 | } 118 | } 119 | 120 | TEST(ConditionVariableTest, wait_for_timeout) { 121 | { 122 | uint64_t actual_waiting_time; 123 | EXPECT_FALSE(wait_for_case(100, 10, &actual_waiting_time)); 124 | EXPECT_GE(actual_waiting_time, 10); 125 | EXPECT_LE(actual_waiting_time, 100); 126 | } 127 | } 128 | -------------------------------------------------------------------------------- /src/base/intrusive_list.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2012-2020 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file List.hpp 36 | * 37 | * An intrusive linked list. 38 | */ 39 | 40 | #pragma once 41 | 42 | namespace uorb { 43 | template 44 | class List; 45 | 46 | template 47 | class ListNode { 48 | friend List; 49 | 50 | private: 51 | void set_next(T sibling) { list_node_sibling_ = sibling; } 52 | T get_next() const { return list_node_sibling_; } 53 | T list_node_sibling_{nullptr}; 54 | }; 55 | 56 | template 57 | class List { 58 | public: 59 | bool Add(T new_node) { 60 | if (!new_node) { 61 | return false; 62 | } 63 | new_node->set_next(head_); 64 | head_ = new_node; 65 | return true; 66 | } 67 | 68 | bool Remove(T remove_node) { 69 | if (!remove_node || !head_) { 70 | return false; 71 | } 72 | 73 | // base case 74 | if (remove_node == head_) { 75 | head_ = head_->get_next(); 76 | return true; 77 | } 78 | 79 | for (T node : *this) { 80 | if (node->get_next() == remove_node) { 81 | node->set_next(node->get_next()->get_next()); 82 | return true; 83 | } 84 | } 85 | 86 | return false; 87 | } 88 | 89 | struct Iterator { 90 | T node; 91 | explicit Iterator(T v) : node(v) {} 92 | explicit operator T() const { return node; } 93 | explicit operator T &() const { return node; } 94 | inline bool operator!=(const Iterator &rhs) { return node != rhs.node; } 95 | T operator*() const { return node; } 96 | Iterator &operator++() { 97 | if (node) { 98 | node = node->get_next(); 99 | } 100 | 101 | return *this; 102 | } 103 | }; 104 | Iterator begin() { return Iterator(head_); } 105 | Iterator begin() const { return Iterator(head_); } 106 | Iterator end() { return Iterator(nullptr); } 107 | Iterator end() const { return Iterator(nullptr); } 108 | 109 | bool empty() const { return head_ == nullptr; } 110 | 111 | unsigned size() const { 112 | unsigned sz = 0; 113 | for (auto node : *this) sz++; 114 | return sz; 115 | } 116 | 117 | void DeleteNode(T node) { 118 | if (Remove(node)) { 119 | // only delete if node was successfully removed 120 | delete node; 121 | } 122 | } 123 | 124 | void DeleteAllNode() { 125 | while (head_) { 126 | auto next = head_->get_next(); 127 | delete head_; 128 | head_ = next; 129 | } 130 | } 131 | 132 | protected: 133 | T head_{nullptr}; 134 | }; 135 | 136 | } // namespace uorb 137 | -------------------------------------------------------------------------------- /src/base/mutex.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #pragma once 5 | 6 | //--------------------------------------------------------- 7 | // Reference: 8 | // https://github.com/google/glog/blob/master/src/base/mutex.h 9 | //--------------------------------------------------------- 10 | 11 | #include 12 | 13 | #include "uorb/internal/noncopyable.h" 14 | 15 | namespace uorb { 16 | namespace base { 17 | 18 | /// The standard Mutex type. 19 | class Mutex : internal::Noncopyable { 20 | #define SAFE_PTHREAD_MUTEX(fncall) \ 21 | do { /* run fncall if is_safe_ is true */ \ 22 | if (is_safe_) fncall(&mutex_); \ 23 | } while (0) 24 | 25 | public: 26 | #ifdef PTHREAD_MUTEX_INITIALIZER 27 | constexpr Mutex() noexcept = default; 28 | ~Mutex() = default; 29 | #else 30 | Mutex() noexcept { 31 | SetIsSafe(); 32 | pthread_mutex_init(&mutex_, nullptr); 33 | } 34 | ~Mutex() noexcept { SAFE_PTHREAD_MUTEX(pthread_mutex_destroy); } 35 | #endif 36 | Mutex(const Mutex &) = delete; 37 | Mutex &operator=(const Mutex &) = delete; 38 | 39 | void lock() { SAFE_PTHREAD_MUTEX(pthread_mutex_lock); } 40 | void unlock() { SAFE_PTHREAD_MUTEX(pthread_mutex_unlock); } 41 | 42 | bool try_lock() noexcept { 43 | return is_safe_ ? 0 == pthread_mutex_trylock(&mutex_) : true; 44 | } 45 | 46 | pthread_mutex_t *native_handle() noexcept { return &mutex_; } 47 | 48 | private: 49 | #ifdef PTHREAD_MUTEX_INITIALIZER 50 | pthread_mutex_t mutex_ = PTHREAD_MUTEX_INITIALIZER; 51 | volatile bool is_safe_{true}; 52 | #else 53 | pthread_mutex_t mutex_{}; 54 | 55 | // We want to make sure that the compiler sets is_safe_ to true only 56 | // when we tell it to, and never makes assumptions is_safe_ is 57 | // always true. volatile is the most reliable way to do that. 58 | volatile bool is_safe_{}; 59 | inline void SetIsSafe() { is_safe_ = true; } 60 | #endif 61 | }; 62 | 63 | /** 64 | * @brief A simple scoped lock type. 65 | * 66 | * A LockGuard controls Mutex ownership within a scope, releasing 67 | * ownership in the destructor. 68 | */ 69 | template 70 | class LockGuard { 71 | public: 72 | explicit LockGuard(MutexType &m) : mutex_(m) { mutex_.lock(); } 73 | ~LockGuard() { mutex_.unlock(); } 74 | 75 | LockGuard(const LockGuard &) = delete; 76 | LockGuard &operator=(const LockGuard &) = delete; 77 | 78 | private: 79 | MutexType &mutex_; 80 | }; 81 | 82 | } // namespace base 83 | } // namespace uorb 84 | -------------------------------------------------------------------------------- /src/base/orb_errno.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #include "base/orb_errno.h" 5 | 6 | #include 7 | 8 | static pthread_key_t key; 9 | static pthread_once_t init_done = PTHREAD_ONCE_INIT; 10 | static int errno_aux; 11 | 12 | static void free_errno(void *p_errno) { 13 | delete reinterpret_cast(p_errno); 14 | } 15 | 16 | static void thread_init() { pthread_key_create(&key, free_errno); } 17 | 18 | int *__orb_errno_location() { 19 | pthread_once(&init_done, thread_init); 20 | 21 | int *p_errno = reinterpret_cast(pthread_getspecific(key)); 22 | 23 | if (p_errno == nullptr) { 24 | p_errno = new int; 25 | } 26 | 27 | if (p_errno != nullptr) { 28 | pthread_setspecific(key, p_errno); 29 | } else { 30 | p_errno = &errno_aux; 31 | } 32 | 33 | return p_errno; 34 | } 35 | -------------------------------------------------------------------------------- /src/base/orb_errno.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #pragma once 5 | 6 | #if defined(__unix__) 7 | #include 8 | #define orb_errno errno 9 | #else 10 | #ifndef ENOENT 11 | #define ENOENT 2 /* No such file or directory */ 12 | #endif 13 | 14 | #ifndef EINTR 15 | #define EINTR 4 /* Interrupted system call */ 16 | #endif 17 | 18 | #ifndef EIO 19 | #define EIO 5 /* I/O error */ 20 | #endif 21 | 22 | #ifndef EBADF 23 | #define EBADF 9 /* Bad file number */ 24 | #endif 25 | 26 | #ifndef EAGAIN 27 | #define EAGAIN 11 /* Try again */ 28 | #endif 29 | 30 | #ifndef ENOMEM 31 | #define ENOMEM 12 /* Out of memory */ 32 | #endif 33 | 34 | #ifndef EFAULT 35 | #define EFAULT 14 /* Bad address */ 36 | #endif 37 | 38 | #ifndef EEXIST 39 | #define EEXIST 17 /* File exists */ 40 | #endif 41 | 42 | #ifndef ENODEV 43 | #define ENODEV 19 /* No such device */ 44 | #endif 45 | 46 | #ifndef EINVAL 47 | #define EINVAL 22 /* Invalid argument */ 48 | #endif 49 | 50 | #ifndef ENFILE 51 | #define ENFILE 23 /* File table overflow */ 52 | #endif 53 | 54 | #ifndef ENOSPC 55 | #define ENOSPC 28 /* No space left on device */ 56 | #endif 57 | 58 | #ifndef ENAMETOOLONG 59 | #define ENAMETOOLONG 36 /* File name too long */ 60 | #endif 61 | 62 | /* 63 | * This error code is special: arch syscall entry code will return 64 | * -ENOSYS if users try to call a syscall that doesn't exist. To keep 65 | * failures of syscalls that really do exist distinguishable from 66 | * failures due to attempts to use a nonexistent syscall, syscall 67 | * implementations should refrain from returning -ENOSYS. 68 | */ 69 | #ifndef ENOSYS 70 | #define ENOSYS 38 /* Invalid system call number */ 71 | #endif 72 | 73 | #ifndef ETIMEDOUT 74 | #define ETIMEDOUT 110 /* Connection timed out */ 75 | #endif 76 | 77 | /* The error code set by various library functions. */ 78 | #if defined(__cplusplus) 79 | extern "C" int *__orb_errno_location(); 80 | #else 81 | extern int *__orb_errno_location(); 82 | #endif 83 | 84 | #define orb_errno (*__orb_errno_location()) 85 | #endif 86 | -------------------------------------------------------------------------------- /src/base/rw_mutex.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #pragma once 5 | 6 | //--------------------------------------------------------- 7 | // Reference: 8 | // https://github.com/google/glog/blob/master/src/base/mutex.h 9 | //--------------------------------------------------------- 10 | 11 | #include 12 | 13 | namespace uorb { 14 | namespace base { 15 | 16 | // The read/write mutex type. 17 | class RwMutex { 18 | #define SAFE_PTHREAD_MUTEX(fncall) \ 19 | do { /* run fncall if is_safe_ is true */ \ 20 | if (is_safe_) fncall(&mutex_); \ 21 | } while (0) 22 | 23 | public: 24 | #ifdef PTHREAD_RWLOCK_INITIALIZER 25 | constexpr RwMutex() noexcept = default; 26 | ~RwMutex() = default; 27 | #else 28 | RwMutex() noexcept { 29 | SetIsSafe(); 30 | pthread_rwlock_init((&mutex_), nullptr); 31 | } 32 | ~RwMutex() noexcept { SAFE_PTHREAD_MUTEX(pthread_rwlock_destroy); } 33 | #endif 34 | RwMutex(const RwMutex &) = delete; 35 | RwMutex &operator=(const RwMutex &) = delete; 36 | 37 | void lock() { SAFE_PTHREAD_MUTEX(pthread_rwlock_wrlock); } 38 | void unlock() { SAFE_PTHREAD_MUTEX(pthread_rwlock_unlock); } 39 | 40 | bool try_lock() noexcept { 41 | return !is_safe_ || 0 == pthread_rwlock_trywrlock(&mutex_); 42 | } 43 | 44 | void reader_lock() { SAFE_PTHREAD_MUTEX(pthread_rwlock_rdlock); } 45 | void reader_unlock() { SAFE_PTHREAD_MUTEX(pthread_rwlock_unlock); } 46 | 47 | #undef SAFE_PTHREAD_MUTEX 48 | 49 | private: 50 | #ifdef PTHREAD_RWLOCK_INITIALIZER 51 | pthread_rwlock_t mutex_ = PTHREAD_RWLOCK_INITIALIZER; 52 | volatile bool is_safe_{true}; 53 | #else 54 | pthread_rwlock_t mutex_{}; 55 | 56 | // We want to make sure that the compiler sets is_safe_ to true only 57 | // when we tell it to, and never makes assumptions is_safe_ is 58 | // always true. volatile is the most reliable way to do that. 59 | volatile bool is_safe_{}; 60 | inline void SetIsSafe() { is_safe_ = true; } 61 | #endif 62 | }; 63 | 64 | /** 65 | * @brief A simple scoped lock type. 66 | * 67 | * A LockGuard controls RwMutex ownership within a scope, releasing 68 | * ownership in the destructor. 69 | */ 70 | class ReaderLockGuard { 71 | public: 72 | explicit ReaderLockGuard(RwMutex &m) : mutex_(m) { mutex_.reader_lock(); } 73 | ~ReaderLockGuard() { mutex_.reader_unlock(); } 74 | 75 | ReaderLockGuard(const ReaderLockGuard &) = delete; 76 | ReaderLockGuard &operator=(const ReaderLockGuard &) = delete; 77 | 78 | private: 79 | RwMutex &mutex_; 80 | }; 81 | 82 | class WriterLockGuard { 83 | public: 84 | explicit WriterLockGuard(RwMutex &m) : mutex_(m) { mutex_.lock(); } 85 | ~WriterLockGuard() { mutex_.unlock(); } 86 | 87 | WriterLockGuard(const WriterLockGuard &) = delete; 88 | WriterLockGuard &operator=(const WriterLockGuard &) = delete; 89 | 90 | private: 91 | RwMutex &mutex_; 92 | }; 93 | 94 | } // namespace base 95 | } // namespace uorb 96 | -------------------------------------------------------------------------------- /src/callback.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #pragma once 5 | 6 | #include "base/condition_variable.h" 7 | 8 | namespace uorb { 9 | 10 | namespace detail { 11 | 12 | struct CallbackBase { 13 | virtual void Notify() = 0; 14 | 15 | private: 16 | virtual void operator()() = 0; 17 | }; 18 | 19 | } // namespace detail 20 | 21 | template 22 | struct Callback : detail::CallbackBase { 23 | private: 24 | void operator()() override = 0; 25 | void Notify() final { operator()(); } 26 | }; 27 | 28 | template <> 29 | struct Callback : detail::CallbackBase { 30 | private: 31 | void operator()() override = 0; 32 | void Notify() final { 33 | base::LockGuard lg(mutex); 34 | operator()(); 35 | } 36 | base::Mutex mutex{}; 37 | }; 38 | 39 | struct SemaphoreCallback : public Callback<>, public base::SimpleSemaphore { 40 | void operator()() override { release(); } 41 | }; 42 | 43 | } // namespace uorb 44 | -------------------------------------------------------------------------------- /src/device_master.cc: -------------------------------------------------------------------------------- 1 | #include "device_master.h" 2 | 3 | #include "device_node.h" 4 | 5 | uorb::DeviceMaster uorb::DeviceMaster::instance_; 6 | 7 | uorb::DeviceNode *uorb::DeviceMaster::CreateAdvertiser(const orb_metadata &meta, 8 | unsigned int *instance) { 9 | const bool is_single_instance = !instance; 10 | const unsigned max_group_tries = 11 | is_single_instance ? 1 : ORB_MULTI_MAX_INSTANCES; 12 | 13 | DeviceNode *device_node; 14 | unsigned group_tries = 0; 15 | 16 | base::LockGuard lg(lock_); 17 | 18 | // Find the following devices that can advertise: 19 | // - Unadvertised device 20 | // - Single instance device 21 | // - Unregistered device 22 | do { 23 | device_node = GetDeviceNodeLocked(meta, group_tries); 24 | if (device_node && 25 | (!device_node->publisher_count() || is_single_instance)) { 26 | device_node->add_publisher(); 27 | break; // Find a unadvertised device or single instance device 28 | } 29 | 30 | if (!device_node) { 31 | device_node = new DeviceNode(meta, group_tries); 32 | if (!device_node) { 33 | errno = ENOMEM; 34 | return nullptr; 35 | } 36 | device_node->add_publisher(); 37 | node_list_.Add(device_node); 38 | break; // Create new device 39 | } 40 | group_tries++; 41 | } while (group_tries < max_group_tries); 42 | 43 | // All instances already exist 44 | if (group_tries >= max_group_tries) { 45 | errno = EEXIST; 46 | return nullptr; 47 | } 48 | 49 | if (instance) *instance = group_tries; 50 | return device_node; 51 | } 52 | 53 | uorb::DeviceNode *uorb::DeviceMaster::GetDeviceNode(const orb_metadata &meta, 54 | uint8_t instance) const { 55 | base::LockGuard lg(lock_); 56 | return GetDeviceNodeLocked(meta, instance); 57 | } 58 | 59 | uorb::DeviceNode *uorb::DeviceMaster::GetDeviceNodeLocked( 60 | const orb_metadata &meta, uint8_t instance) const { 61 | // We can safely return the node that can be used by any thread, because a 62 | // DeviceNode never gets deleted. 63 | for (auto node : node_list_) { 64 | if (node->IsSameWith(meta, instance)) return node; 65 | } 66 | 67 | return nullptr; 68 | } 69 | 70 | uorb::DeviceNode *uorb::DeviceMaster::OpenDeviceNode(const orb_metadata &meta, 71 | unsigned int instance) { 72 | if (instance >= ORB_MULTI_MAX_INSTANCES) { 73 | return nullptr; 74 | } 75 | 76 | base::LockGuard lg(lock_); 77 | 78 | DeviceNode *device_node = GetDeviceNodeLocked(meta, instance); 79 | if (device_node) { 80 | return device_node; 81 | } 82 | 83 | device_node = new DeviceNode(meta, instance); 84 | 85 | if (!device_node) { 86 | errno = ENOMEM; 87 | return nullptr; 88 | } 89 | 90 | node_list_.Add(device_node); 91 | 92 | return device_node; // Create new device 93 | } 94 | -------------------------------------------------------------------------------- /src/device_master.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "base/intrusive_list.h" 7 | #include "base/mutex.h" 8 | #include "uorb/uorb.h" 9 | 10 | namespace uorb { 11 | class DeviceNode; 12 | class DeviceMaster; 13 | } // namespace uorb 14 | 15 | /** 16 | * Master control device for uorb message device node. 17 | */ 18 | class uorb::DeviceMaster { 19 | public: 20 | /** 21 | * Method to get the singleton instance for the uorb::DeviceMaster. 22 | * @return DeviceMaster instance reference 23 | */ 24 | static inline DeviceMaster &get_instance() { return instance_; } 25 | 26 | /** 27 | * Advertise as the publisher of a topic 28 | * @param meta The uORB metadata (usually from the *ORB_ID() macro) for the 29 | * topic. 30 | * @param instance Pointer to an integer which will yield the instance ID 31 | * (0-based) of the publication. This is an output parameter and will be set 32 | * to the newly created instance, ie. 0 for the first advertiser, 1 for the 33 | * next and so on. If it is nullptr, it will only be created at 0. 34 | * @return nullptr on error, and set errno to orb_errno. Otherwise returns a 35 | * DeviceNode that can be used to publish to the topic. 36 | */ 37 | DeviceNode *CreateAdvertiser(const orb_metadata &meta, 38 | unsigned int *instance); 39 | 40 | DeviceNode *OpenDeviceNode(const orb_metadata &meta, unsigned int instance); 41 | 42 | /** 43 | * Public interface for GetDeviceNodeLocked(). Takes care of synchronization. 44 | * @return node if exists, nullptr otherwise 45 | */ 46 | DeviceNode *GetDeviceNode(const orb_metadata &meta, uint8_t instance) const; 47 | 48 | private: 49 | /** 50 | * Find a node give its name. 51 | * lock_ must already be held when calling this. 52 | * @return node if exists, nullptr otherwise 53 | */ 54 | DeviceNode *GetDeviceNodeLocked(const orb_metadata &meta, 55 | uint8_t instance) const; 56 | 57 | // Private constructor, uorb::Manager takes care of its creation 58 | DeviceMaster() = default; 59 | ~DeviceMaster() = default; 60 | 61 | static DeviceMaster instance_; 62 | 63 | List node_list_{}; 64 | mutable base::Mutex lock_{}; 65 | }; 66 | -------------------------------------------------------------------------------- /src/device_node.cc: -------------------------------------------------------------------------------- 1 | #include "device_node.h" 2 | 3 | #include 4 | #include 5 | 6 | static inline uint16_t RoundPowOfTwo(uint16_t n) { 7 | if (n == 0) { 8 | return 1; 9 | } 10 | 11 | // Avoid is already a power of 2 12 | uint32_t value = n - 1; 13 | 14 | // Fill 1 15 | value |= value >> 1U; 16 | value |= value >> 2U; 17 | value |= value >> 4U; 18 | value |= value >> 8U; 19 | 20 | // Unable to round-up, take the value of round-down 21 | if (value == UINT16_MAX) { 22 | value >>= 1U; 23 | } 24 | 25 | return value + 1; 26 | } 27 | 28 | uorb::DeviceNode::DeviceNode(const struct orb_metadata &meta, uint8_t instance) 29 | : meta_(meta), 30 | instance_(instance), 31 | queue_size_(RoundPowOfTwo(meta.o_queue_size)) {} 32 | 33 | uorb::DeviceNode::~DeviceNode() { delete[] data_; } 34 | 35 | bool uorb::DeviceNode::Copy(void *dst, unsigned *sub_generation_ptr) const { 36 | if (!dst || !sub_generation_ptr || !data_) { 37 | return false; 38 | } 39 | 40 | auto &sub_generation = *sub_generation_ptr; 41 | 42 | base::LockGuard lg(lock_); 43 | 44 | // If queue_size is 4 and cur_generation is 10, then 6, 7, 8, 9 are in the 45 | // range, and others are not. 46 | 47 | // The subscriber already read the latest message, but nothing new was 48 | // published yet. Return the previous message */ 49 | if (generation_ == sub_generation) { 50 | sub_generation = generation_ - 1; 51 | } else if (generation_ - sub_generation > queue_size_) { 52 | // Reader is too far behind: some messages are lost 53 | sub_generation = generation_ - queue_size_; 54 | } 55 | 56 | memcpy(dst, data_ + (meta_.o_size * (sub_generation & (queue_size_ - 1))), 57 | meta_.o_size); 58 | 59 | ++sub_generation; 60 | 61 | return true; 62 | } 63 | 64 | unsigned uorb::DeviceNode::updates_available(unsigned generation) const { 65 | return generation_ - generation; 66 | } 67 | 68 | bool uorb::DeviceNode::Publish(const void *data) { 69 | if (data == nullptr) { 70 | errno = EFAULT; 71 | return false; 72 | } 73 | 74 | base::LockGuard lg(lock_); 75 | 76 | if (nullptr == data_) { 77 | data_ = new uint8_t[meta_.o_size * queue_size_]; 78 | 79 | /* failed or could not allocate */ 80 | if (nullptr == data_) { 81 | errno = ENOMEM; 82 | return false; 83 | } 84 | } 85 | 86 | memcpy(data_ + (meta_.o_size * (generation_ % queue_size_)), 87 | (const char *)data, meta_.o_size); 88 | 89 | generation_++; 90 | 91 | for (auto callback : callbacks_) { 92 | (*callback).Notify(); 93 | } 94 | 95 | return true; 96 | } 97 | 98 | void uorb::DeviceNode::add_subscriber() { 99 | base::LockGuard lg(lock_); 100 | subscriber_count_++; 101 | } 102 | 103 | void uorb::DeviceNode::remove_subscriber() { 104 | base::LockGuard lg(lock_); 105 | subscriber_count_--; 106 | } 107 | 108 | unsigned uorb::DeviceNode::initial_generation() const { 109 | base::LockGuard lg(lock_); 110 | 111 | // If there any previous publications allow the subscriber to read them 112 | return generation_ - (data_ ? 1 : 0); 113 | } 114 | 115 | void uorb::DeviceNode::remove_publisher() { 116 | base::LockGuard lg(lock_); 117 | publisher_count_--; 118 | } 119 | 120 | void uorb::DeviceNode::add_publisher() { 121 | base::LockGuard lg(lock_); 122 | publisher_count_++; 123 | } 124 | -------------------------------------------------------------------------------- /src/device_node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "base/condition_variable.h" 10 | #include "base/intrusive_list.h" 11 | #include "base/mutex.h" 12 | #include "callback.h" 13 | 14 | namespace uORBTest { 15 | class UnitTest; 16 | } 17 | 18 | namespace uorb { 19 | class DeviceMaster; 20 | 21 | /** 22 | * Per-object device instance. 23 | */ 24 | class DeviceNode : public ListNode, 25 | private internal::Noncopyable { 26 | friend DeviceMaster; 27 | 28 | public: 29 | // Publish a data to this node. 30 | bool Publish(const void *data); 31 | 32 | void add_subscriber(); 33 | void remove_subscriber(); 34 | uint8_t subscriber_count() const { return subscriber_count_; } 35 | bool has_anonymous_subscriber() const { return has_anonymous_subscriber_; } 36 | void mark_anonymous_subscriber() { has_anonymous_subscriber_ = true; } 37 | 38 | void add_publisher(); 39 | void remove_publisher(); 40 | uint8_t publisher_count() const { return publisher_count_; } 41 | bool has_anonymous_publisher() const { return has_anonymous_publisher_; } 42 | void mark_anonymous_publisher() { has_anonymous_publisher_ = true; } 43 | 44 | // Whether meta and instance are the same as the current one 45 | inline bool IsSameWith(const orb_metadata &meta, uint8_t instance) const { 46 | return IsSameWith(meta) && (instance_ == instance); 47 | } 48 | 49 | inline bool IsSameWith(const orb_metadata &meta) const { 50 | return &meta_ == &meta; 51 | } 52 | 53 | // add item to list of work items to schedule on node update 54 | template 55 | bool RegisterCallback(Callback *callback) { 56 | if (!callback) { 57 | errno = EINVAL; 58 | return false; 59 | } 60 | 61 | uorb::base::LockGuard lg(lock_); 62 | uorb::DeviceNode::callbacks_.emplace(callback); 63 | return true; 64 | } 65 | 66 | // remove item from list of work items 67 | template 68 | bool UnregisterCallback(Callback *callback) { 69 | base::LockGuard lg(lock_); 70 | return uorb::DeviceNode::callbacks_.erase(callback) != 0; 71 | } 72 | 73 | // Returns the number of updated data relative to the parameter 'generation' 74 | unsigned updates_available(unsigned generation) const; 75 | unsigned initial_generation() const; 76 | 77 | unsigned queue_size() const { return queue_size_; } 78 | 79 | const char *name() const { return meta_.o_name; } 80 | uint8_t instance() const { return instance_; } 81 | 82 | /** 83 | * Copies data and the corresponding generation 84 | * from a node to the buffer provided. 85 | * 86 | * @param dst 87 | * The buffer into which the data is copied. 88 | * @param sub_generation 89 | * The generation that was copied. 90 | * @return bool 91 | * Returns true if the data was copied. 92 | */ 93 | bool Copy(void *dst, unsigned *sub_generation) const; 94 | 95 | private: 96 | friend uORBTest::UnitTest; 97 | 98 | const orb_metadata &meta_; /**< object metadata information */ 99 | const uint8_t instance_; /**< orb multi instance identifier */ 100 | 101 | uint8_t *data_{nullptr}; /**< allocated object buffer */ 102 | const uint16_t queue_size_; /**< maximum number of elements in the queue */ 103 | unsigned generation_{0}; /**< object generation count */ 104 | 105 | mutable base::Mutex lock_{}; 106 | 107 | uint8_t subscriber_count_{0}; 108 | bool has_anonymous_subscriber_{false}; 109 | uint8_t publisher_count_{0}; 110 | bool has_anonymous_publisher_{false}; 111 | 112 | std::set callbacks_; 113 | 114 | DeviceNode(const struct orb_metadata &meta, uint8_t instance); 115 | ~DeviceNode(); 116 | }; 117 | } // namespace uorb 118 | -------------------------------------------------------------------------------- /src/git_version.cc.in: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | 5 | #include "uorb/uorb.h" 6 | 7 | const char *orb_version() { return "@UORB_GIT_TAG@"; } -------------------------------------------------------------------------------- /src/subscription_impl.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #pragma once 5 | 6 | #include "device_node.h" 7 | 8 | namespace uorb { 9 | 10 | struct SubscriptionImpl { 11 | explicit SubscriptionImpl(DeviceNode &device_node) : dev_(device_node) { 12 | last_generation_ = device_node.initial_generation(); 13 | dev_.add_subscriber(); 14 | } 15 | 16 | ~SubscriptionImpl() { dev_.remove_subscriber(); } 17 | 18 | bool Copy(void *buffer) { return dev_.Copy(buffer, &last_generation_); } 19 | unsigned updates_available() const { 20 | return dev_.updates_available(last_generation_); 21 | } 22 | 23 | template 24 | bool UnregisterCallback(Callback *callback) { 25 | return dev_.UnregisterCallback(callback); 26 | } 27 | 28 | template 29 | bool RegisterCallback(Callback *callback) { 30 | return dev_.RegisterCallback(callback); 31 | } 32 | 33 | private: 34 | DeviceNode &dev_; 35 | unsigned last_generation_{}; /**< last generation the subscriber has seen */ 36 | }; 37 | } // namespace uorb 38 | -------------------------------------------------------------------------------- /src/uorb.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * @file uorb.cpp 3 | * A lightweight object broker. 4 | */ 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include "callback.h" 11 | #include "device_master.h" 12 | #include "device_node.h" 13 | #include "subscription_impl.h" 14 | 15 | using uorb::DeviceMaster; 16 | using uorb::DeviceNode; 17 | using uorb::SubscriptionImpl; 18 | 19 | #define ORB_CHECK_TRUE(condition, error_code, error_action) \ 20 | ({ \ 21 | if (!static_cast(condition)) { \ 22 | errno = error_code; \ 23 | error_action; \ 24 | } \ 25 | }) 26 | 27 | orb_publication_t *orb_create_publication(const struct orb_metadata *meta) { 28 | return orb_create_publication_multi(meta, nullptr); 29 | } 30 | 31 | orb_publication_t *orb_create_publication_multi(const struct orb_metadata *meta, 32 | unsigned int *instance) { 33 | ORB_CHECK_TRUE(meta, EINVAL, return nullptr); 34 | auto &meta_ = *meta; 35 | auto &device_master = DeviceMaster::get_instance(); 36 | auto *dev_ = device_master.CreateAdvertiser(meta_, instance); 37 | 38 | ORB_CHECK_TRUE(dev_, ENOMEM, return nullptr); 39 | 40 | return reinterpret_cast(dev_); 41 | } 42 | 43 | bool orb_destroy_publication(orb_publication_t **handle_ptr) { 44 | ORB_CHECK_TRUE(handle_ptr && *handle_ptr, EINVAL, return false); 45 | 46 | auto &publication_handle = *handle_ptr; 47 | 48 | auto &dev = *(uorb::DeviceNode *)publication_handle; 49 | dev.remove_publisher(); 50 | 51 | publication_handle = nullptr; 52 | 53 | return true; 54 | } 55 | 56 | bool orb_publish(orb_publication_t *handle, const void *data) { 57 | ORB_CHECK_TRUE(handle && data, EINVAL, return false); 58 | 59 | auto &dev = *(uorb::DeviceNode *)handle; 60 | return dev.Publish(data); 61 | } 62 | 63 | bool orb_publish_anonymous(const struct orb_metadata *meta, const void *data) { 64 | ORB_CHECK_TRUE(meta, EINVAL, return false); 65 | 66 | auto &device_master = DeviceMaster::get_instance(); 67 | auto *dev = device_master.OpenDeviceNode(*meta, 0); 68 | ORB_CHECK_TRUE(dev, ENOMEM, return false); 69 | 70 | // Mark as an anonymous publisher, then copy the latest data 71 | dev->mark_anonymous_publisher(); 72 | return dev->Publish(data); 73 | } 74 | 75 | orb_subscription_t *orb_create_subscription(const struct orb_metadata *meta) { 76 | return orb_create_subscription_multi(meta, 0); 77 | } 78 | 79 | orb_subscription_t *orb_create_subscription_multi( 80 | const struct orb_metadata *meta, unsigned instance) { 81 | ORB_CHECK_TRUE(meta, EINVAL, return nullptr); 82 | 83 | DeviceMaster &device_master = uorb::DeviceMaster::get_instance(); 84 | 85 | auto *dev = device_master.OpenDeviceNode(*meta, instance); 86 | ORB_CHECK_TRUE(dev, ENOMEM, return nullptr); 87 | 88 | // Create a subscriber, if it fails, we don't have to release device_node (it 89 | // only increases but not decreases) 90 | auto *subscriber = new SubscriptionImpl(*dev); 91 | ORB_CHECK_TRUE(subscriber, ENOMEM, return nullptr); 92 | 93 | return reinterpret_cast(subscriber); 94 | } 95 | 96 | bool orb_destroy_subscription(orb_subscription_t **handle_ptr) { 97 | ORB_CHECK_TRUE(handle_ptr && *handle_ptr, EINVAL, return false); 98 | 99 | auto &subscription_handle = *handle_ptr; 100 | 101 | delete reinterpret_cast(subscription_handle); 102 | subscription_handle = nullptr; // Set the original pointer to null 103 | 104 | return true; 105 | } 106 | 107 | bool orb_copy(orb_subscription_t *handle, void *buffer) { 108 | ORB_CHECK_TRUE(handle && buffer, EINVAL, return false); 109 | 110 | auto &sub = *reinterpret_cast(handle); 111 | 112 | return sub.Copy(buffer); 113 | } 114 | 115 | bool orb_copy_anonymous(const struct orb_metadata *meta, void *buffer) { 116 | ORB_CHECK_TRUE(meta, EINVAL, return false); 117 | 118 | auto &device_master = DeviceMaster::get_instance(); 119 | auto *dev = device_master.OpenDeviceNode(*meta, 0); 120 | ORB_CHECK_TRUE(dev, ENOMEM, return false); 121 | 122 | // Mark as anonymous subscription, then copy the latest data 123 | dev->mark_anonymous_subscriber(); 124 | unsigned last_generation_ = dev->initial_generation(); 125 | return dev->Copy(buffer, &last_generation_); 126 | } 127 | 128 | bool orb_check_update(orb_subscription_t *handle) { 129 | ORB_CHECK_TRUE(handle, EINVAL, return false); 130 | 131 | auto &sub = *reinterpret_cast(handle); 132 | 133 | return sub.updates_available(); 134 | } 135 | 136 | bool orb_exists(const struct orb_metadata *meta, unsigned int instance) { 137 | ORB_CHECK_TRUE(meta, EINVAL, return false); 138 | 139 | auto &master = DeviceMaster::get_instance(); 140 | auto *dev = master.GetDeviceNode(*meta, instance); 141 | 142 | return dev && dev->publisher_count(); 143 | } 144 | 145 | unsigned int orb_group_count(const struct orb_metadata *meta) { 146 | ORB_CHECK_TRUE(meta, EINVAL, return false); 147 | 148 | unsigned int instance = 0; 149 | 150 | for (unsigned int i = 0; i < ORB_MULTI_MAX_INSTANCES; ++i) { 151 | if (orb_exists(meta, i)) { 152 | ++instance; 153 | } 154 | } 155 | return instance; 156 | } 157 | 158 | bool orb_get_topic_status(const struct orb_metadata *meta, 159 | unsigned int instance, struct orb_status *status) { 160 | ORB_CHECK_TRUE(meta, EINVAL, return false); 161 | 162 | auto &master = DeviceMaster::get_instance(); 163 | auto *dev = master.GetDeviceNode(*meta, instance); 164 | 165 | if (!dev) return false; 166 | 167 | if (status) { 168 | status->queue_size = dev->queue_size(); 169 | status->subscriber_count = dev->subscriber_count(); 170 | status->has_anonymous_subscriber = dev->has_anonymous_subscriber(); 171 | status->publisher_count = dev->publisher_count(); 172 | status->has_anonymous_publisher = dev->has_anonymous_publisher(); 173 | status->latest_data_index = dev->updates_available(0); 174 | } 175 | return true; 176 | } 177 | 178 | int orb_poll(struct orb_pollfd *fds, unsigned int nfds, int timeout_ms) { 179 | ORB_CHECK_TRUE(fds && nfds, EINVAL, return -1); 180 | 181 | int updated_num = 0; // Number of new messages 182 | uorb::SemaphoreCallback semaphore_callback; 183 | 184 | for (unsigned i = 0; i < nfds; ++i) { 185 | auto &item = fds[i]; 186 | if (!item.fd) { 187 | continue; 188 | } 189 | 190 | auto &item_sub = *reinterpret_cast(item.fd); 191 | item_sub.RegisterCallback(&semaphore_callback); 192 | 193 | if (item_sub.updates_available() > 0) { 194 | ++updated_num; 195 | } 196 | } 197 | 198 | // No new data, waiting for update 199 | if (updated_num == 0) { 200 | if (timeout_ms > 0) { 201 | semaphore_callback.try_acquire_for(timeout_ms); 202 | } else if (timeout_ms < 0) { 203 | semaphore_callback.acquire(); 204 | } 205 | } else { 206 | updated_num = 0; 207 | } 208 | 209 | for (unsigned i = 0; i < nfds; ++i) { 210 | auto &item = fds[i]; 211 | if (!item.fd) { 212 | continue; 213 | } 214 | 215 | auto &item_sub = *reinterpret_cast(item.fd); 216 | item_sub.UnregisterCallback(&semaphore_callback); 217 | 218 | item.revents = 0; 219 | if (item_sub.updates_available()) { 220 | item.revents |= item.events & POLLIN; 221 | ++updated_num; 222 | } 223 | } 224 | 225 | return updated_num; 226 | } 227 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(uorb_test) 2 | 3 | # pthread 4 | find_package(Threads REQUIRED) 5 | link_libraries(Threads::Threads) 6 | 7 | # Generate orb message 8 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/msg) 9 | 10 | # uorb test source 11 | aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} TEST_SOURCE) 12 | add_executable(${PROJECT_NAME} ${TEST_SOURCE}) 13 | target_link_libraries(${PROJECT_NAME} PRIVATE GTest::gtest_main) 14 | target_link_libraries(${PROJECT_NAME} PRIVATE uorb_unittests_msgs) 15 | target_include_directories(${PROJECT_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../src) 16 | 17 | add_test(${PROJECT_NAME} ${PROJECT_NAME}) 18 | -------------------------------------------------------------------------------- /tests/msg/.gitignore: -------------------------------------------------------------------------------- 1 | *.[ch]pp 2 | *.cc 3 | *.[ch] 4 | -------------------------------------------------------------------------------- /tests/msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(uorb_unittests_msgs LANGUAGES NONE) 2 | cmake_minimum_required(VERSION 3.12...4.0) 3 | 4 | # Support IN_LIST if() operator 5 | cmake_policy(SET CMP0057 NEW) 6 | 7 | find_package (Python3 COMPONENTS Interpreter Development) 8 | # We have a custom error message to tell users how to install python3. 9 | if (NOT Python3_Interpreter_FOUND) 10 | message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n" 11 | " Ubuntu: sudo apt install python3 python3-dev python3-pip\n" 12 | " macOS: brew install python") 13 | endif () 14 | 15 | file(GLOB msg_files 16 | *.msg 17 | ) 18 | 19 | # headers 20 | set(msg_out_path ${CMAKE_CURRENT_SOURCE_DIR}/uorb/topics) 21 | set(msg_source_out_path ${CMAKE_CURRENT_SOURCE_DIR}/uorb/topics_sources) 22 | 23 | set(uorb_headers) 24 | set(uorb_sources ${msg_source_out_path}/uorb_topics.cc) 25 | foreach (msg_file ${msg_files}) 26 | get_filename_component(msg ${msg_file} NAME_WE) 27 | list(APPEND uorb_headers ${msg_out_path}/${msg}.h) 28 | list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cc) 29 | endforeach () 30 | 31 | set(tools_root ${CMAKE_CURRENT_SOURCE_DIR}/../../tools/msg) 32 | 33 | # Generate uORB headers 34 | add_custom_command(OUTPUT ${uorb_headers} 35 | COMMAND ${PYTHON_EXECUTABLE} ${tools_root}/tools/px_generate_uorb_topic_files.py 36 | --headers 37 | -f ${msg_files} 38 | -i ${CMAKE_CURRENT_SOURCE_DIR} 39 | -o ${msg_out_path} 40 | -e ${tools_root}/templates/uorb 41 | -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers 42 | -q 43 | DEPENDS 44 | ${msg_files} 45 | ${tools_root}/templates/uorb/msg.h.em 46 | ${tools_root}/tools/px_generate_uorb_topic_files.py 47 | COMMENT "Generating uORB topic headers" 48 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 49 | VERBATIM 50 | ) 51 | 52 | # Generate uORB sources 53 | add_custom_command(OUTPUT ${uorb_sources} 54 | COMMAND ${PYTHON_EXECUTABLE} ${tools_root}/tools/px_generate_uorb_topic_files.py 55 | --sources 56 | -f ${msg_files} 57 | -i ${CMAKE_CURRENT_SOURCE_DIR} 58 | -o ${msg_source_out_path} 59 | -e ${tools_root}/templates/uorb 60 | -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources 61 | -q 62 | ${added_arguments} 63 | DEPENDS 64 | ${msg_files} 65 | ${tools_root}/templates/uorb/msg.cc.em 66 | ${tools_root}/tools/px_generate_uorb_topic_files.py 67 | COMMENT "Generating uORB topic sources" 68 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 69 | VERBATIM 70 | ) 71 | 72 | add_library(${PROJECT_NAME} ${uorb_sources} ${uorb_headers}) 73 | target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 74 | target_link_libraries(${PROJECT_NAME} PUBLIC uorb) 75 | -------------------------------------------------------------------------------- /tests/msg/orb_test.msg: -------------------------------------------------------------------------------- 1 | uint64 timestamp # time since system start (microseconds) 2 | 3 | int32 val 4 | 5 | # TOPICS orb_test orb_multitest 6 | -------------------------------------------------------------------------------- /tests/msg/orb_test_large.msg: -------------------------------------------------------------------------------- 1 | uint64 timestamp # time since system start (microseconds) 2 | 3 | int32 val 4 | 5 | uint8[512] junk 6 | -------------------------------------------------------------------------------- /tests/msg/orb_test_medium.msg: -------------------------------------------------------------------------------- 1 | uint64 timestamp # time since system start (microseconds) 2 | 3 | int32 val 4 | 5 | uint8[64] junk 6 | 7 | uint16 ORB_QUEUE_SIZE = 16 8 | 9 | # TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue 10 | -------------------------------------------------------------------------------- /tests/msg/orb_test_queue_poll.msg: -------------------------------------------------------------------------------- 1 | uint64 timestamp # time since system start (microseconds) 2 | 3 | int32 val 4 | 5 | uint8[64] junk 6 | 7 | uint16 ORB_QUEUE_SIZE = 16 8 | -------------------------------------------------------------------------------- /tests/slog.h: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 shawnfeng. All rights reserved. 3 | // 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | // Precompiler define to get only filename; 10 | #if !defined(__FILENAME__) 11 | #define __FILENAME__ \ 12 | ({ \ 13 | strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 \ 14 | : strrchr(__FILE__, '\\') ? strrchr(__FILE__, '\\') + 1 \ 15 | : __FILE__; \ 16 | }) 17 | #endif 18 | 19 | #define LOGGER_LEVEL_TRACE "T" 20 | #define LOGGER_LEVEL_DEBUG "D" 21 | #define LOGGER_LEVEL_INFO "I" 22 | #define LOGGER_LEVEL_WARN "W" 23 | #define LOGGER_LEVEL_ERROR "E" 24 | #define LOGGER_LEVEL_FATAL "F" 25 | 26 | #define LOGGER_RESET_COLOR "\x1b[0m" 27 | #define LOGGER_TRACE_COLOR "\x1b[37m" 28 | #define LOGGER_DEBUG_COLOR "\x1b[34m" 29 | #define LOGGER_INFO_COLOR "\x1b[32m" 30 | #define LOGGER_WARN_COLOR "\x1b[33m" 31 | #define LOGGER_ERROR_COLOR "\x1b[31m" 32 | #define LOGGER_FATAL_COLOR "\x1b[35m" 33 | 34 | #define LOGGER_OUT(level, fmt, ...) \ 35 | do { \ 36 | printf("%s%s/(%s:%d %s) %s" fmt LOGGER_RESET_COLOR "\r\n", \ 37 | LOGGER_RESET_COLOR, LOGGER_LEVEL_##level, __FILENAME__, __LINE__, \ 38 | __FUNCTION__, LOGGER_##level##_COLOR, ##__VA_ARGS__); \ 39 | } while (0) 40 | 41 | #define LOGGER_TRACE(fmt, ...) LOGGER_OUT(TRACE, fmt, ##__VA_ARGS__) 42 | #define LOGGER_DEBUG(fmt, ...) LOGGER_OUT(DEBUG, fmt, ##__VA_ARGS__) 43 | #define LOGGER_INFO(fmt, ...) LOGGER_OUT(INFO, fmt, ##__VA_ARGS__) 44 | #define LOGGER_WARN(fmt, ...) LOGGER_OUT(WARN, fmt, ##__VA_ARGS__) 45 | #define LOGGER_ERROR(fmt, ...) LOGGER_OUT(ERROR, fmt, ##__VA_ARGS__) 46 | #define LOGGER_FATAL(fmt, ...) LOGGER_OUT(FATAL, fmt, ##__VA_ARGS__) 47 | -------------------------------------------------------------------------------- /tests/uorb_unit_test.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | 49 | #include "device_master.h" 50 | #include "device_node.h" 51 | #include "slog.h" 52 | 53 | #define ORB_DEBUG LOGGER_INFO 54 | #define ORB_INFO LOGGER_INFO 55 | #define ORB_ERROR LOGGER_ERROR 56 | 57 | namespace uORBTest { 58 | class UnitTest; 59 | } 60 | 61 | class uORBTest::UnitTest : public testing::Test { 62 | public: 63 | // Assist in testing the wrap-around situation 64 | static void set_generation(uorb::DeviceNode &node, unsigned generation) { 65 | node.generation_ = generation; 66 | } 67 | 68 | template 69 | void latency_test(const orb_metadata *T); 70 | 71 | // Disallow copy 72 | UnitTest(const uORBTest::UnitTest & /*unused*/) = delete; 73 | void SetUp() override {} 74 | void TearDown() override {} 75 | void TestBody() override {} 76 | UnitTest() = default; 77 | }; 78 | 79 | template 80 | void uORBTest::UnitTest::latency_test(const orb_metadata *T) { 81 | S pub_data{}; 82 | pub_data.val = 308; 83 | pub_data.timestamp = orb_absolute_time_us(); 84 | 85 | orb_publication_t *pfd0 = orb_create_publication(T); 86 | ASSERT_NE(pfd0, nullptr) << "orb_advertise failed: " << errno; 87 | 88 | orb_publish(pfd0, &pub_data); 89 | 90 | bool pub_sub_test_passed = false; 91 | 92 | /* test pub / sub latency */ 93 | 94 | // Can'pub_data pass a pointer in args, must be a null terminated 95 | // array of strings because the strings are copied to 96 | // prevent access if the caller data goes out of scope 97 | std::thread pub_sub_latency_thread{[&]() { 98 | /* poll on test topic and output latency */ 99 | float latency_integral = 0.0f; 100 | 101 | /* wakeup source(s) */ 102 | orb_pollfd_t fds[3]; 103 | 104 | auto test_multi_sub = orb_create_subscription(ORB_ID(orb_test)); 105 | auto test_multi_sub_medium = 106 | orb_create_subscription(ORB_ID(orb_test_medium)); 107 | auto test_multi_sub_large = orb_create_subscription(ORB_ID(orb_test_large)); 108 | 109 | orb_test_large_s pub_data_large{}; 110 | 111 | /* clear all ready flags */ 112 | orb_copy(test_multi_sub, &pub_data_large); 113 | orb_copy(test_multi_sub_medium, &pub_data_large); 114 | orb_copy(test_multi_sub_large, &pub_data_large); 115 | 116 | fds[0].fd = test_multi_sub; 117 | fds[0].events = POLLIN; 118 | fds[1].fd = test_multi_sub_medium; 119 | fds[1].events = POLLIN; 120 | fds[2].fd = test_multi_sub_large; 121 | fds[2].events = POLLIN; 122 | 123 | const unsigned max_runs = 1000; 124 | int current_value = pub_data_large.val; 125 | int num_missed = 0; 126 | 127 | // timings has to be on the heap to keep frame size below 2048 bytes 128 | auto *timings = new unsigned[max_runs]{}; 129 | unsigned timing_min = 9999999, timing_max = 0; 130 | 131 | for (unsigned i = 0; i < max_runs; i++) { 132 | /* wait for up to 500ms for data */ 133 | int pret = orb_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); 134 | 135 | if (fds[0].revents & POLLIN) { 136 | orb_copy(test_multi_sub, &pub_data_large); 137 | 138 | } else if (fds[1].revents & POLLIN) { 139 | orb_copy(test_multi_sub_medium, &pub_data_large); 140 | 141 | } else if (fds[2].revents & POLLIN) { 142 | orb_copy(test_multi_sub_large, &pub_data_large); 143 | } 144 | 145 | if (pret < 0) { 146 | ORB_ERROR("poll error %d, %d", pret, errno); 147 | continue; 148 | } 149 | 150 | num_missed += pub_data_large.val - current_value - 1; 151 | current_value = pub_data_large.val; 152 | 153 | auto elt = (unsigned)orb_elapsed_time_us(pub_data_large.timestamp); 154 | latency_integral += elt; 155 | timings[i] = elt; 156 | 157 | if (elt > timing_max) { 158 | timing_max = elt; 159 | } 160 | 161 | if (elt < timing_min) { 162 | timing_min = elt; 163 | } 164 | } 165 | 166 | orb_destroy_subscription(&test_multi_sub); 167 | orb_destroy_subscription(&test_multi_sub_medium); 168 | orb_destroy_subscription(&test_multi_sub_large); 169 | 170 | float std_dev = 0.f; 171 | float mean = latency_integral / max_runs; 172 | 173 | for (unsigned i = 0; i < max_runs; i++) { 174 | float diff = (float)timings[i] - mean; 175 | std_dev += diff * diff; 176 | } 177 | 178 | delete[] timings; 179 | 180 | ORB_INFO("uORB version: %s", orb_version()); 181 | ORB_INFO("mean: %8.4f us", static_cast(mean)); 182 | ORB_INFO("std dev: %8.4f us", 183 | static_cast(sqrtf(std_dev / (max_runs - 1)))); 184 | ORB_INFO("min: %3i us", timing_min); 185 | ORB_INFO("max: %3i us", timing_max); 186 | ORB_INFO("missed topic updates: %i", num_missed); 187 | 188 | pub_sub_test_passed = true; 189 | 190 | ASSERT_LT(static_cast(latency_integral / max_runs), 200.0f); 191 | }}; 192 | /* give the test task some data */ 193 | while (!pub_sub_test_passed) { 194 | ++pub_data.val; 195 | pub_data.timestamp = orb_absolute_time_us(); 196 | 197 | ASSERT_TRUE(orb_publish(pfd0, &pub_data)) << "mult. pub0 timing fail"; 198 | 199 | /* simulate >800 Hz system operation */ 200 | usleep(1000); 201 | } 202 | 203 | orb_destroy_publication(&pfd0); 204 | 205 | pub_sub_latency_thread.join(); 206 | } 207 | -------------------------------------------------------------------------------- /toolchains/himix200.toolchain.cmake: -------------------------------------------------------------------------------- 1 | # set cross-compiled system type, it's better not use the type which cmake cannot recognized. 2 | SET(CMAKE_SYSTEM_NAME Linux) 3 | SET(CMAKE_SYSTEM_PROCESSOR arm) 4 | 5 | # when hislicon SDK was installed, toolchain was installed in the path as below: 6 | SET(CMAKE_FIND_ROOT_PATH "/opt/hisi-linux/x86-arm/arm-himix200-linux") 7 | SET(CMAKE_C_COMPILER "arm-himix200-linux-gcc") 8 | SET(CMAKE_CXX_COMPILER "arm-himix200-linux-g++") 9 | 10 | # set searching rules for cross-compiler 11 | SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 12 | SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 13 | SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 14 | 15 | # set ${CMAKE_C_FLAGS} and ${CMAKE_CXX_FLAGS}flag for cross-compiled process 16 | SET(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 17 | SET(CMAKE_C_FLAGS "-std=gnu11 ${CMAKE_C_FLAGS}") 18 | -------------------------------------------------------------------------------- /toolchains/hisiv500.toolchain.cmake: -------------------------------------------------------------------------------- 1 | # set cross-compiled system type, it's better not use the type which cmake cannot recognized. 2 | SET(CMAKE_SYSTEM_NAME Linux) 3 | SET(CMAKE_SYSTEM_PROCESSOR arm) 4 | 5 | # when hislicon SDK was installed, toolchain was installed in the path as below: 6 | SET(CMAKE_FIND_ROOT_PATH "/opt/hisi-linux/x86-arm/arm-hisiv500-linux/target") 7 | SET(CMAKE_C_COMPILER "arm-hisiv500-linux-gcc") 8 | SET(CMAKE_CXX_COMPILER "arm-hisiv500-linux-g++") 9 | 10 | # set searching rules for cross-compiler 11 | SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 12 | SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 13 | SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 14 | 15 | # set ${CMAKE_C_FLAGS} and ${CMAKE_CXX_FLAGS}flag for cross-compiled process 16 | SET(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 17 | SET(CMAKE_C_FLAGS "-std=gnu11 ${CMAKE_C_FLAGS}") 18 | -------------------------------------------------------------------------------- /toolchains/host.gcc.toolchain.cmake: -------------------------------------------------------------------------------- 1 | # set cross-compiled system type, it's better not use the type which cmake cannot recognized. 2 | SET(CMAKE_SYSTEM_NAME Linux) 3 | SET(CMAKE_SYSTEM_PROCESSOR x86) 4 | 5 | # if gcc/g++ was installed: 6 | SET(CMAKE_C_COMPILER "gcc") 7 | SET(CMAKE_CXX_COMPILER "g++") 8 | 9 | # set searching rules 10 | SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 11 | SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 12 | SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 13 | 14 | # set ${CMAKE_C_FLAGS} and ${CMAKE_CXX_FLAGS}flag 15 | # c11 does not contain clock_* functions, but gnu11 contains 16 | SET(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 17 | SET(CMAKE_C_FLAGS "-std=gnu11 ${CMAKE_C_FLAGS}") 18 | -------------------------------------------------------------------------------- /tools/msg/templates/uorb/msg.cc.em: -------------------------------------------------------------------------------- 1 | @############################################### 2 | @# 3 | @# PX4 ROS compatible message source code 4 | @# generation for C++ 5 | @# 6 | @# EmPy template for generating .h files 7 | @# Based on the original template for ROS 8 | @# 9 | @############################################### 10 | @# Start of Template 11 | @# 12 | @# Context: 13 | @# - file_name_in (String) Source file 14 | @# - spec (msggen.MsgSpec) Parsed specification of the .msg file 15 | @# - md5sum (String) MD5Sum of the .msg specification 16 | @# - search_path (dict) search paths for genmsg 17 | @# - topics (List of String) multi-topic names 18 | @# - constrained_flash set to true if flash is constrained 19 | @# - ids (List) list of all RTPS msg ids 20 | @############################################### 21 | /**************************************************************************** 22 | * 23 | * Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. 24 | * 25 | * Redistribution and use in source and binary forms, with or without 26 | * modification, are permitted provided that the following conditions 27 | * are met: 28 | * 29 | * 1. Redistributions of source code must retain the above copyright 30 | * notice, this list of conditions and the following disclaimer. 31 | * 2. Redistributions in binary form must reproduce the above copyright 32 | * notice, this list of conditions and the following disclaimer in 33 | * the documentation and/or other materials provided with the 34 | * distribution. 35 | * 3. Neither the name PX4 nor the names of its contributors may be 36 | * used to endorse or promote products derived from this software 37 | * without specific prior written permission. 38 | * 39 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 40 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 41 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 42 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 43 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 44 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 45 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 46 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 47 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 48 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 49 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 50 | * POSSIBILITY OF SUCH DAMAGE. 51 | * 52 | ****************************************************************************/ 53 | 54 | @{ 55 | import genmsg.msgs 56 | 57 | from px_generate_uorb_topic_helper import * # this is in Tools/ 58 | 59 | uorb_struct = '%s_s'%spec.short_name 60 | topic_name = spec.short_name 61 | 62 | sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) 63 | struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) 64 | topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields] 65 | 66 | topic_queue_size = 1 67 | for constant in spec.constants: 68 | if constant.name == "ORB_QUEUE_SIZE": 69 | topic_queue_size = constant.val 70 | break 71 | }@ 72 | 73 | #include 74 | 75 | @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed" 76 | @# This is used for the logger 77 | static constexpr char orb_@(topic_name)_fields[] = 78 | "@( ";".join(topic_fields) );"; 79 | 80 | @[for multi_topic in topics]@ 81 | ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), orb_@(topic_name)_fields, @topic_queue_size); 82 | @[end for] 83 | -------------------------------------------------------------------------------- /tools/msg/templates/uorb/msg.h.em: -------------------------------------------------------------------------------- 1 | @############################################### 2 | @# 3 | @# PX4 ROS compatible message source code 4 | @# generation for C++ 5 | @# 6 | @# EmPy template for generating .h files 7 | @# Based on the original template for ROS 8 | @# 9 | @############################################### 10 | @# Start of Template 11 | @# 12 | @# Context: 13 | @# - file_name_in (String) Source file 14 | @# - spec (msggen.MsgSpec) Parsed specification of the .msg file 15 | @# - md5sum (String) MD5Sum of the .msg specification 16 | @# - search_path (dict) search paths for genmsg 17 | @# - topics (List of String) multi-topic names 18 | @# - ids (List) list of all RTPS msg ids 19 | @############################################### 20 | /**************************************************************************** 21 | * 22 | * Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. 23 | * 24 | * Redistribution and use in source and binary forms, with or without 25 | * modification, are permitted provided that the following conditions 26 | * are met: 27 | * 28 | * 1. Redistributions of source code must retain the above copyright 29 | * notice, this list of conditions and the following disclaimer. 30 | * 2. Redistributions in binary form must reproduce the above copyright 31 | * notice, this list of conditions and the following disclaimer in 32 | * the documentation and/or other materials provided with the 33 | * distribution. 34 | * 3. Neither the name PX4 nor the names of its contributors may be 35 | * used to endorse or promote products derived from this software 36 | * without specific prior written permission. 37 | * 38 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 39 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 40 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 41 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 42 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 43 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 44 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 45 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 46 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 47 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 48 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 49 | * POSSIBILITY OF SUCH DAMAGE. 50 | * 51 | ****************************************************************************/ 52 | 53 | @{ 54 | import genmsg.msgs 55 | 56 | from px_generate_uorb_topic_helper import * # this is in Tools/ 57 | 58 | uorb_struct = '%s_s'%spec.short_name 59 | uorb_struct_upper = spec.short_name.upper() 60 | topic_name = spec.short_name 61 | }@ 62 | 63 | #pragma once 64 | 65 | @############################## 66 | @# Generic Includes 67 | @############################## 68 | 69 | #include 70 | #include 71 | 72 | @############################## 73 | @# Includes for dependencies 74 | @############################## 75 | @{ 76 | for field in spec.parsed_fields(): 77 | if (not field.is_builtin): 78 | if (not field.is_header): 79 | (package, name) = genmsg.names.package_resource_name(field.base_type) 80 | package = package or spec.package # convert '' to package 81 | print('#include '%(name)) 82 | }@ 83 | 84 | @# Constants c style 85 | #ifndef __cplusplus 86 | @[for constant in spec.constants]@ 87 | #define @(uorb_struct_upper)_@(constant.name) @(int(constant.val)) 88 | @[end for] 89 | #endif 90 | 91 | @############################## 92 | @# Main struct of message 93 | @############################## 94 | @{ 95 | 96 | def print_parsed_fields(): 97 | # sort fields (using a stable sort) 98 | sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) 99 | struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) 100 | # loop over all fields and print the type and name 101 | for field in sorted_fields: 102 | if (not field.is_header): 103 | print_field_def(field) 104 | }@ 105 | 106 | #ifdef __cplusplus 107 | @#class @(uorb_struct) { 108 | struct __EXPORT @(uorb_struct) { 109 | @#public: 110 | #else 111 | struct @(uorb_struct) { 112 | #endif 113 | @print_parsed_fields() 114 | 115 | #ifdef __cplusplus 116 | @# Constants again c++-ified 117 | @{ 118 | for constant in spec.constants: 119 | type_name = constant.type 120 | if type_name in type_map: 121 | # need to add _t: int8 --> int8_t 122 | type_px4 = type_map[type_name] 123 | else: 124 | raise Exception("Type {0} not supported, add to to template file!".format(type_name)) 125 | 126 | print(' static constexpr %s %s = %s;'%(type_px4, constant.name, int(constant.val))) 127 | } 128 | #endif 129 | }; 130 | 131 | /* register this as object request broker structure */ 132 | @[for multi_topic in topics]@ 133 | ORB_DECLARE(@multi_topic, @uorb_struct); 134 | @[end for] 135 | -------------------------------------------------------------------------------- /tools/msg/templates/uorb/uorb_topics.cc.em: -------------------------------------------------------------------------------- 1 | @############################################### 2 | @# 3 | @# EmPy template for generating uorb_topics.cc file 4 | @# for logging purposes 5 | @# 6 | @############################################### 7 | @# Start of Template 8 | @# 9 | @# Context: 10 | @# - msgs (List) list of all msg files 11 | @# - multi_topics (List) list of all multi-topic names 12 | @# - ids (List) list of all RTPS msg ids 13 | @############################################### 14 | /**************************************************************************** 15 | * 16 | * Copyright (C) 2013-2020 PX4 Development Team. All rights reserved. 17 | * 18 | * Redistribution and use in source and binary forms, with or without 19 | * modification, are permitted provided that the following conditions 20 | * are met: 21 | * 22 | * 1. Redistributions of source code must retain the above copyright 23 | * notice, this list of conditions and the following disclaimer. 24 | * 2. Redistributions in binary form must reproduce the above copyright 25 | * notice, this list of conditions and the following disclaimer in 26 | * the documentation and/or other materials provided with the 27 | * distribution. 28 | * 3. Neither the name PX4 nor the names of its contributors may be 29 | * used to endorse or promote products derived from this software 30 | * without specific prior written permission. 31 | * 32 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 33 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 34 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 35 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 36 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 37 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 38 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 39 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 40 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 41 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 42 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 43 | * POSSIBILITY OF SUCH DAMAGE. 44 | * 45 | ****************************************************************************/ 46 | 47 | #include 48 | #include 49 | @{ 50 | msg_names = [mn.replace(".msg", "") for mn in msgs] 51 | msgs_count = len(msg_names) 52 | msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates 53 | msg_names_all.sort() 54 | msgs_count_all = len(msg_names_all) 55 | }@ 56 | @[for msg_name in msg_names]@ 57 | #include 58 | @[end for] 59 | 60 | const constexpr struct orb_metadata *const uorb_topics_list[] = { 61 | @[for idx, msg_name in enumerate(msg_names_all, 1)]@ 62 | &uorb::msg::@(msg_name)@[if idx != msgs_count_all],@[end if] 63 | @[end for]}; 64 | 65 | const struct orb_metadata *const *orb_get_topics(size_t *size) { 66 | if (size) *size = sizeof(uorb_topics_list)/sizeof(uorb_topics_list[0]); 67 | return uorb_topics_list; 68 | } 69 | -------------------------------------------------------------------------------- /tools/msg/templates/uorb/uorb_topics.h.em: -------------------------------------------------------------------------------- 1 | @############################################### 2 | @# 3 | @# EmPy template for generating uORBTopics.hpp file 4 | @# for logging purposes 5 | @# 6 | @############################################### 7 | @# Start of Template 8 | @# 9 | @# Context: 10 | @# - msgs (List) list of all msg files 11 | @# - multi_topics (List) list of all multi-topic names 12 | @# - ids (List) list of all RTPS msg ids 13 | @############################################### 14 | /**************************************************************************** 15 | * 16 | * Copyright (C) 2020 PX4 Development Team. All rights reserved. 17 | * 18 | * Redistribution and use in source and binary forms, with or without 19 | * modification, are permitted provided that the following conditions 20 | * are met: 21 | * 22 | * 1. Redistributions of source code must retain the above copyright 23 | * notice, this list of conditions and the following disclaimer. 24 | * 2. Redistributions in binary form must reproduce the above copyright 25 | * notice, this list of conditions and the following disclaimer in 26 | * the documentation and/or other materials provided with the 27 | * distribution. 28 | * 3. Neither the name PX4 nor the names of its contributors may be 29 | * used to endorse or promote products derived from this software 30 | * without specific prior written permission. 31 | * 32 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 33 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 34 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 35 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 36 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 37 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 38 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 39 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 40 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 41 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 42 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 43 | * POSSIBILITY OF SUCH DAMAGE. 44 | * 45 | ****************************************************************************/ 46 | 47 | @{ 48 | msg_names = [mn.replace(".msg", "") for mn in msgs] 49 | msgs_count = len(msg_names) 50 | msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates 51 | msg_names_all.sort() 52 | msgs_count_all = len(msg_names_all) 53 | }@ 54 | 55 | #pragma once 56 | 57 | #include 58 | 59 | #include 60 | 61 | #ifdef __cplusplus 62 | static constexpr size_t ORB_TOPICS_COUNT{@(msgs_count_all)}; 63 | static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } 64 | #endif 65 | 66 | #ifdef __cplusplus 67 | extern "C" { 68 | #endif 69 | 70 | /* 71 | * Returns array of topics metadata 72 | */ 73 | const struct orb_metadata *const *orb_get_topics(size_t *size) __EXPORT; 74 | 75 | #ifdef __cplusplus 76 | } 77 | #endif 78 | -------------------------------------------------------------------------------- /tools/msg/tools/genmsg/__init__.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2011, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | from . base import MSG_DIR, SRV_DIR, EXT_MSG, EXT_SRV, SEP, log, plog, InvalidMsgSpec, log_verbose, MsgGenerationException 34 | from . gentools import compute_md5, compute_full_text, compute_md5_text 35 | from . names import resource_name_base, package_resource_name, is_legal_resource_base_name, \ 36 | resource_name_package, resource_name, is_legal_resource_name 37 | from . msgs import HEADER, TIME, DURATION, MsgSpec, Constant, Field 38 | from . msg_loader import MsgNotFound, MsgContext, load_depends, load_msg_by_type, load_srv_by_type 39 | from . srvs import SrvSpec 40 | 41 | -------------------------------------------------------------------------------- /tools/msg/tools/genmsg/base.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2011, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | from __future__ import print_function 34 | import os, sys 35 | 36 | SEP = '/' 37 | 38 | MSG_DIR = 'msg' 39 | SRV_DIR = 'srv' 40 | 41 | EXT_MSG = '.msg' 42 | EXT_SRV = '.srv' 43 | 44 | ## character that designates a constant assignment rather than a field 45 | CONSTCHAR = '=' 46 | COMMENTCHAR = '#' 47 | IODELIM = '---' 48 | 49 | 50 | verbose = False 51 | 52 | import inspect, pprint 53 | 54 | def log_verbose(value): 55 | global verbose 56 | verbose = value 57 | 58 | def log(*args): 59 | global verbose 60 | if verbose: 61 | print("%s:%d" % inspect.stack()[1][1:3], file=sys.stderr) 62 | print(' '.join([str(x) for x in args]), file=sys.stderr) 63 | 64 | def plog(msg, obj): 65 | if verbose: 66 | print("%s:%d" % inspect.stack()[1][1:3], file=sys.stderr) 67 | print(msg, " ", file=sys.stderr) 68 | pprint.pprint(obj, file=sys.stderr) 69 | 70 | class InvalidMsgSpec(Exception): 71 | pass 72 | 73 | class MsgGenerationException(Exception): 74 | pass 75 | 76 | -------------------------------------------------------------------------------- /tools/msg/tools/genmsg/command_line.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2011, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | def includepath_to_dict(includepath): 34 | search_path = {} 35 | if includepath: 36 | for path in includepath: 37 | key = path[:path.find(':')] 38 | value = path[path.find(':')+1:] 39 | if value: 40 | search_path.setdefault(key, []).append(value) 41 | return search_path 42 | -------------------------------------------------------------------------------- /tools/msg/tools/genmsg/deps.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2011, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | import os 34 | import genmsg.msg_loader 35 | import genmsg 36 | 37 | # pkg_name - string 38 | # msg_file - string full path 39 | # search_paths - dict of {'pkg':'msg_dir'} 40 | def find_msg_dependencies_with_type(pkg_name, msg_file, search_paths): 41 | 42 | # Read and parse the source msg file 43 | msg_context = genmsg.msg_loader.MsgContext.create_default() 44 | full_type_name = genmsg.gentools.compute_full_type_name(pkg_name, os.path.basename(msg_file)) 45 | spec = genmsg.msg_loader.load_msg_from_file(msg_context, msg_file, full_type_name) 46 | 47 | try: 48 | genmsg.msg_loader.load_depends(msg_context, spec, search_paths) 49 | except genmsg.InvalidMsgSpec as e: 50 | raise genmsg.MsgGenerationException("Cannot read .msg for %s: %s"%(full_type_name, str(e))) 51 | 52 | deps = set() 53 | for dep_type_name in msg_context.get_all_depends(full_type_name): 54 | deps.add((dep_type_name, msg_context.get_file(dep_type_name))) 55 | 56 | return list(deps) 57 | 58 | 59 | def find_msg_dependencies(pkg_name, msg_file, search_paths): 60 | deps = find_msg_dependencies_with_type(pkg_name, msg_file, search_paths) 61 | return [d[1] for d in deps] 62 | 63 | 64 | def find_srv_dependencies_with_type(pkg_name, msg_file, search_paths): 65 | 66 | # Read and parse the source msg file 67 | msg_context = genmsg.msg_loader.MsgContext.create_default() 68 | full_type_name = genmsg.gentools.compute_full_type_name(pkg_name, os.path.basename(msg_file)) 69 | 70 | spec = genmsg.msg_loader.load_srv_from_file(msg_context, msg_file, full_type_name) 71 | 72 | try: 73 | genmsg.msg_loader.load_depends(msg_context, spec, search_paths) 74 | except genmsg.InvalidMsgSpec as e: 75 | raise genmsg.MsgGenerationException("Cannot read .msg for %s: %s"%(full_type_name, str(e))) 76 | 77 | deps = set() 78 | 79 | for dep_type_name in msg_context.get_all_depends(spec.request.full_name): 80 | deps.add((dep_type_name, msg_context.get_file(dep_type_name))) 81 | 82 | for dep_type_name in msg_context.get_all_depends(spec.response.full_name): 83 | deps.add((dep_type_name, msg_context.get_file(dep_type_name))) 84 | 85 | return list(deps) 86 | 87 | 88 | def find_srv_dependencies(pkg_name, msg_file, search_paths): 89 | deps = find_srv_dependencies_with_type(pkg_name, msg_file, search_paths) 90 | return [d[1] for d in deps] 91 | 92 | #paths = {'std_msgs':'/u/mkjargaard/repositories/mkjargaard/dist-sandbox/std_msgs/msg'} 93 | #file = '/u/mkjargaard/repositories/mkjargaard/dist-sandbox/quux_msgs/msg/QuuxString.msg' 94 | #find_msg_dependencies('quux_msgs', file, paths) 95 | -------------------------------------------------------------------------------- /tools/msg/tools/genmsg/gentools.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 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 | Library for supporting message and service generation for all ROS 36 | client libraries. This is mainly responsible for calculating the 37 | md5sums and message definitions of classes. 38 | """ 39 | 40 | # NOTE: this should not contain any rospy-specific code. The rospy 41 | # generator library is rospy.genpy. 42 | 43 | import sys 44 | import hashlib 45 | 46 | try: 47 | from cStringIO import StringIO # Python 2.x 48 | except ImportError: 49 | from io import StringIO # Python 3.x 50 | 51 | from . import msgs 52 | 53 | from .msgs import InvalidMsgSpec, MsgSpec, bare_msg_type, is_builtin 54 | from .msg_loader import load_depends 55 | from .srvs import SrvSpec 56 | from . import names 57 | from . import base 58 | 59 | def compute_md5_text(msg_context, spec): 60 | """ 61 | Compute the text used for md5 calculation. MD5 spec states that we 62 | removes comments and non-meaningful whitespace. We also strip 63 | packages names from type names. For convenience sake, constants are 64 | reordered ahead of other declarations, in the order that they were 65 | originally defined. 66 | 67 | :returns: text for ROS MD5-processing, ``str`` 68 | """ 69 | package = spec.package 70 | 71 | buff = StringIO() 72 | 73 | for c in spec.constants: 74 | buff.write("%s %s=%s\n"%(c.type, c.name, c.val_text)) 75 | for type_, name in zip(spec.types, spec.names): 76 | msg_type = bare_msg_type(type_) 77 | # md5 spec strips package names 78 | if is_builtin(msg_type): 79 | buff.write("%s %s\n"%(type_, name)) 80 | else: 81 | # recursively generate md5 for subtype. have to build up 82 | # dependency representation for subtype in order to 83 | # generate md5 84 | sub_pkg, _ = names.package_resource_name(msg_type) 85 | sub_pkg = sub_pkg or package 86 | sub_spec = msg_context.get_registered(msg_type) 87 | sub_md5 = compute_md5(msg_context, sub_spec) 88 | buff.write("%s %s\n"%(sub_md5, name)) 89 | 90 | return buff.getvalue().strip() # remove trailing new line 91 | 92 | def _compute_hash(msg_context, spec, hash): 93 | """ 94 | subroutine of compute_md5() 95 | 96 | :param msg_context: :class:`MsgContext` instance to load dependencies into/from. 97 | :param spec: :class:`MsgSpec` to compute hash for. 98 | :param hash: hash instance 99 | """ 100 | # accumulate the hash 101 | # - root file 102 | if isinstance(spec, MsgSpec): 103 | hash.update(compute_md5_text(msg_context, spec).encode()) 104 | elif isinstance(spec, SrvSpec): 105 | hash.update(compute_md5_text(msg_context, spec.request).encode()) 106 | hash.update(compute_md5_text(msg_context, spec.response).encode()) 107 | else: 108 | raise Exception("[%s] is not a message or service"%spec) 109 | return hash.hexdigest() 110 | 111 | def compute_md5(msg_context, spec): 112 | """ 113 | Compute md5 hash for message/service 114 | 115 | :param msg_context: :class:`MsgContext` instance to load dependencies into/from. 116 | :param spec: :class:`MsgSpec` to compute md5 for. 117 | :returns: md5 hash, ``str`` 118 | """ 119 | return _compute_hash(msg_context, spec, hashlib.md5()) 120 | 121 | ## alias 122 | compute_md5_v2 = compute_md5 123 | 124 | def _unique_deps(dep_list): 125 | uniques = [] 126 | for d in dep_list: 127 | if d not in uniques: 128 | uniques.append(d) 129 | return uniques 130 | 131 | def compute_full_text(msg_context, spec): 132 | """ 133 | Compute full text of message/service, including text of embedded 134 | types. The text of the main msg/srv is listed first. Embedded 135 | msg/srv files are denoted first by an 80-character '=' separator, 136 | followed by a type declaration line,'MSG: pkg/type', followed by 137 | the text of the embedded type. 138 | 139 | :param msg_context: :class:`MsgContext` instance to load dependencies into/from. 140 | :param spec: :class:`MsgSpec` to compute full text for. 141 | :returns: concatenated text for msg/srv file and embedded msg/srv types, ``str`` 142 | """ 143 | buff = StringIO() 144 | sep = '='*80+'\n' 145 | 146 | # write the text of the top-level type 147 | buff.write(spec.text) 148 | buff.write('\n') 149 | # append the text of the dependencies (embedded types). Can't use set() as we have to preserve order. 150 | for d in _unique_deps(msg_context.get_all_depends(spec.full_name)): 151 | buff.write(sep) 152 | buff.write("MSG: %s\n"%d) 153 | buff.write(msg_context.get_registered(d).text) 154 | buff.write('\n') 155 | # #1168: remove the trailing \n separator that is added by the concatenation logic 156 | return buff.getvalue()[:-1] 157 | 158 | def compute_full_type_name(package_name, file_name): 159 | """ 160 | Compute the full type name of message/service 'pkg/type'. 161 | 162 | :param package_name: name of package file is in, ``str`` 163 | :file_name: name of the msg or srv file, ``str`` 164 | :returns: typename in format 'pkg/type' 165 | :raises: :exc:`MsgGenerationException` if file_name ends with an unknown file extension 166 | """ 167 | # strip extension 168 | for ext in (base.EXT_MSG, base.EXT_SRV): 169 | if file_name.endswith(ext): 170 | short_name = file_name[:-len(ext)] 171 | break 172 | else: 173 | raise base.MsgGenerationException("Processing file: '%s' - unknown file extension"% (file_name)) 174 | return "%s/%s"%(package_name, short_name) 175 | 176 | -------------------------------------------------------------------------------- /tools/msg/tools/genmsg/names.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2008, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | PRN_SEPARATOR = '/' 34 | 35 | import re 36 | 37 | def normalize_package_context(package_context): 38 | package_context = package_context.strip() 39 | while package_context.endswith(PRN_SEPARATOR): 40 | package_context = package_context[:-1] 41 | return package_context 42 | 43 | ####################################################################### 44 | # RESOURCE NAMES 45 | # resource names refer to entities in a file system 46 | 47 | def resource_name(res_pkg_name, name, my_pkg=None): 48 | """ 49 | Convert package name + resource into a fully qualified resource name 50 | 51 | @param res_pkg_name: name of package resource is located in 52 | @type res_pkg_name: str 53 | @param name: resource base name 54 | @type name: str 55 | @param my_pkg: name of package resource is being referred to 56 | in. If specified, name will be returned in local form if 57 | res_pkg_name is my_pkg 58 | @type my_pkg: str 59 | @return: name for resource 60 | @rtype: str 61 | """ 62 | if res_pkg_name != my_pkg: 63 | return res_pkg_name+PRN_SEPARATOR+name 64 | return name 65 | 66 | def resource_name_base(name): 67 | """ 68 | pkg/typeName -> typeName, typeName -> typeName 69 | 70 | Convert fully qualified resource name into the package-less resource name 71 | @param name: package resource name, e.g. 'std_msgs/String' 72 | @type name: str 73 | @return: resource name sans package-name scope 74 | @rtype: str 75 | """ 76 | 77 | return name[name.rfind(PRN_SEPARATOR)+1:] 78 | 79 | def resource_name_package(name): 80 | """ 81 | pkg/typeName -> pkg, typeName -> None 82 | 83 | @param name: package resource name, e.g. 'std_msgs/String' 84 | @type name: str 85 | @return: package name of resource 86 | @rtype: str 87 | """ 88 | 89 | if not PRN_SEPARATOR in name: 90 | return None 91 | return name[:name.find(PRN_SEPARATOR)] 92 | 93 | def package_resource_name(name): 94 | """ 95 | Split a name into its package and resource name parts, e.g. 'std_msgs/String -> std_msgs, String' 96 | 97 | @param name: package resource name, e.g. 'std_msgs/String' 98 | @type name: str 99 | @return: package name, resource name 100 | @rtype: str 101 | @raise ValueError: if name is invalid 102 | """ 103 | if PRN_SEPARATOR in name: 104 | val = tuple(name.split(PRN_SEPARATOR)) 105 | if len(val) != 2: 106 | raise ValueError("invalid name [%s]"%name) 107 | else: 108 | return val 109 | else: 110 | return '', name 111 | 112 | ################################################################################ 113 | # NAME VALIDATORS 114 | 115 | #ascii char followed by (alphanumeric, _, /) 116 | RESOURCE_NAME_LEGAL_CHARS_P = re.compile('^[A-Za-z][\w_\/]*$') 117 | def is_legal_resource_name(name): 118 | """ 119 | Check if name is a legal ROS name for filesystem resources 120 | (alphabetical character followed by alphanumeric, underscore, or 121 | forward slashes). This constraint is currently not being enforced, 122 | but may start getting enforced in later versions of ROS. 123 | 124 | @param name: Name 125 | @type name: str 126 | """ 127 | # resource names can be unicode due to filesystem 128 | if name is None: 129 | return False 130 | m = RESOURCE_NAME_LEGAL_CHARS_P.match(name) 131 | # '//' check makes sure there isn't double-slashes 132 | return m is not None and m.group(0) == name and not '//' in name 133 | 134 | BASE_RESOURCE_NAME_LEGAL_CHARS_P = re.compile('^[A-Za-z][\w_]*$') #ascii char followed by (alphanumeric, _) 135 | def is_legal_resource_base_name(name): 136 | """ 137 | Validates that name is a legal resource base name. A base name has 138 | no package context, e.g. "String". 139 | """ 140 | # resource names can be unicode due to filesystem 141 | if name is None: 142 | return False 143 | m = BASE_RESOURCE_NAME_LEGAL_CHARS_P.match(name) 144 | return m is not None and m.group(0) == name 145 | 146 | -------------------------------------------------------------------------------- /tools/msg/tools/genmsg/srvs.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2008, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | """ 34 | ROS Service Description Language Spec 35 | Implements http://ros.org/wiki/srv 36 | """ 37 | 38 | import os 39 | import sys 40 | 41 | from . names import is_legal_resource_name, is_legal_resource_base_name, package_resource_name, resource_name 42 | 43 | class SrvSpec(object): 44 | 45 | def __init__(self, request, response, text, full_name = '', short_name = '', package = ''): 46 | 47 | alt_package, alt_short_name = package_resource_name(full_name) 48 | if not package: 49 | package = alt_package 50 | if not short_name: 51 | short_name = alt_short_name 52 | 53 | self.request = request 54 | self.response = response 55 | self.text = text 56 | self.full_name = full_name 57 | self.short_name = short_name 58 | self.package = package 59 | 60 | 61 | def __eq__(self, other): 62 | if not other or not isinstance(other, SrvSpec): 63 | return False 64 | return self.request == other.request and \ 65 | self.response == other.response and \ 66 | self.text == other.text and \ 67 | self.full_name == other.full_name and \ 68 | self.short_name == other.short_name and \ 69 | self.package == other.package 70 | 71 | def __ne__(self, other): 72 | if not other or not isinstance(other, SrvSpec): 73 | return True 74 | return not self.__eq__(other) 75 | 76 | def __repr__(self): 77 | return "SrvSpec[%s, %s]"%(repr(self.request), repr(self.response)) 78 | 79 | -------------------------------------------------------------------------------- /tools/msg/tools/packaging/__about__.py: -------------------------------------------------------------------------------- 1 | # This file is dual licensed under the terms of the Apache License, Version 2 | # 2.0, and the BSD License. See the LICENSE file in the root of this repository 3 | # for complete details. 4 | 5 | __all__ = [ 6 | "__title__", 7 | "__summary__", 8 | "__uri__", 9 | "__version__", 10 | "__author__", 11 | "__email__", 12 | "__license__", 13 | "__copyright__", 14 | ] 15 | 16 | __title__ = "packaging" 17 | __summary__ = "Core utilities for Python packages" 18 | __uri__ = "https://github.com/pypa/packaging" 19 | 20 | __version__ = "21.3" 21 | 22 | __author__ = "Donald Stufft and individual contributors" 23 | __email__ = "donald@stufft.io" 24 | 25 | __license__ = "BSD-2-Clause or Apache-2.0" 26 | __copyright__ = "2014-2019 %s" % __author__ 27 | -------------------------------------------------------------------------------- /tools/msg/tools/packaging/__init__.py: -------------------------------------------------------------------------------- 1 | # This file is dual licensed under the terms of the Apache License, Version 2 | # 2.0, and the BSD License. See the LICENSE file in the root of this repository 3 | # for complete details. 4 | 5 | from .__about__ import ( 6 | __author__, 7 | __copyright__, 8 | __email__, 9 | __license__, 10 | __summary__, 11 | __title__, 12 | __uri__, 13 | __version__, 14 | ) 15 | 16 | __all__ = [ 17 | "__title__", 18 | "__summary__", 19 | "__uri__", 20 | "__version__", 21 | "__author__", 22 | "__email__", 23 | "__license__", 24 | "__copyright__", 25 | ] 26 | -------------------------------------------------------------------------------- /tools/msg/tools/packaging/_musllinux.py: -------------------------------------------------------------------------------- 1 | """PEP 656 support. 2 | 3 | This module implements logic to detect if the currently running Python is 4 | linked against musl, and what musl version is used. 5 | """ 6 | 7 | import contextlib 8 | import functools 9 | import operator 10 | import os 11 | import re 12 | import struct 13 | import subprocess 14 | import sys 15 | from typing import IO, Iterator, NamedTuple, Optional, Tuple 16 | 17 | 18 | def _read_unpacked(f: IO[bytes], fmt: str) -> Tuple[int, ...]: 19 | return struct.unpack(fmt, f.read(struct.calcsize(fmt))) 20 | 21 | 22 | def _parse_ld_musl_from_elf(f: IO[bytes]) -> Optional[str]: 23 | """Detect musl libc location by parsing the Python executable. 24 | 25 | Based on: https://gist.github.com/lyssdod/f51579ae8d93c8657a5564aefc2ffbca 26 | ELF header: https://refspecs.linuxfoundation.org/elf/gabi4+/ch4.eheader.html 27 | """ 28 | f.seek(0) 29 | try: 30 | ident = _read_unpacked(f, "16B") 31 | except struct.error: 32 | return None 33 | if ident[:4] != tuple(b"\x7fELF"): # Invalid magic, not ELF. 34 | return None 35 | f.seek(struct.calcsize("HHI"), 1) # Skip file type, machine, and version. 36 | 37 | try: 38 | # e_fmt: Format for program header. 39 | # p_fmt: Format for section header. 40 | # p_idx: Indexes to find p_type, p_offset, and p_filesz. 41 | e_fmt, p_fmt, p_idx = { 42 | 1: ("IIIIHHH", "IIIIIIII", (0, 1, 4)), # 32-bit. 43 | 2: ("QQQIHHH", "IIQQQQQQ", (0, 2, 5)), # 64-bit. 44 | }[ident[4]] 45 | except KeyError: 46 | return None 47 | else: 48 | p_get = operator.itemgetter(*p_idx) 49 | 50 | # Find the interpreter section and return its content. 51 | try: 52 | _, e_phoff, _, _, _, e_phentsize, e_phnum = _read_unpacked(f, e_fmt) 53 | except struct.error: 54 | return None 55 | for i in range(e_phnum + 1): 56 | f.seek(e_phoff + e_phentsize * i) 57 | try: 58 | p_type, p_offset, p_filesz = p_get(_read_unpacked(f, p_fmt)) 59 | except struct.error: 60 | return None 61 | if p_type != 3: # Not PT_INTERP. 62 | continue 63 | f.seek(p_offset) 64 | interpreter = os.fsdecode(f.read(p_filesz)).strip("\0") 65 | if "musl" not in interpreter: 66 | return None 67 | return interpreter 68 | return None 69 | 70 | 71 | class _MuslVersion(NamedTuple): 72 | major: int 73 | minor: int 74 | 75 | 76 | def _parse_musl_version(output: str) -> Optional[_MuslVersion]: 77 | lines = [n for n in (n.strip() for n in output.splitlines()) if n] 78 | if len(lines) < 2 or lines[0][:4] != "musl": 79 | return None 80 | m = re.match(r"Version (\d+)\.(\d+)", lines[1]) 81 | if not m: 82 | return None 83 | return _MuslVersion(major=int(m.group(1)), minor=int(m.group(2))) 84 | 85 | 86 | @functools.lru_cache() 87 | def _get_musl_version(executable: str) -> Optional[_MuslVersion]: 88 | """Detect currently-running musl runtime version. 89 | 90 | This is done by checking the specified executable's dynamic linking 91 | information, and invoking the loader to parse its output for a version 92 | string. If the loader is musl, the output would be something like:: 93 | 94 | musl libc (x86_64) 95 | Version 1.2.2 96 | Dynamic Program Loader 97 | """ 98 | with contextlib.ExitStack() as stack: 99 | try: 100 | f = stack.enter_context(open(executable, "rb")) 101 | except OSError: 102 | return None 103 | ld = _parse_ld_musl_from_elf(f) 104 | if not ld: 105 | return None 106 | proc = subprocess.run([ld], stderr=subprocess.PIPE, universal_newlines=True) 107 | return _parse_musl_version(proc.stderr) 108 | 109 | 110 | def platform_tags(arch: str) -> Iterator[str]: 111 | """Generate musllinux tags compatible to the current platform. 112 | 113 | :param arch: Should be the part of platform tag after the ``linux_`` 114 | prefix, e.g. ``x86_64``. The ``linux_`` prefix is assumed as a 115 | prerequisite for the current platform to be musllinux-compatible. 116 | 117 | :returns: An iterator of compatible musllinux tags. 118 | """ 119 | sys_musl = _get_musl_version(sys.executable) 120 | if sys_musl is None: # Python not dynamically linked against musl. 121 | return 122 | for minor in range(sys_musl.minor, -1, -1): 123 | yield f"musllinux_{sys_musl.major}_{minor}_{arch}" 124 | 125 | 126 | if __name__ == "__main__": # pragma: no cover 127 | import sysconfig 128 | 129 | plat = sysconfig.get_platform() 130 | assert plat.startswith("linux-"), "not linux" 131 | 132 | print("plat:", plat) 133 | print("musl:", _get_musl_version(sys.executable)) 134 | print("tags:", end=" ") 135 | for t in platform_tags(re.sub(r"[.-]", "_", plat.split("-", 1)[-1])): 136 | print(t, end="\n ") 137 | -------------------------------------------------------------------------------- /tools/msg/tools/packaging/_structures.py: -------------------------------------------------------------------------------- 1 | # This file is dual licensed under the terms of the Apache License, Version 2 | # 2.0, and the BSD License. See the LICENSE file in the root of this repository 3 | # for complete details. 4 | 5 | 6 | class InfinityType: 7 | def __repr__(self) -> str: 8 | return "Infinity" 9 | 10 | def __hash__(self) -> int: 11 | return hash(repr(self)) 12 | 13 | def __lt__(self, other: object) -> bool: 14 | return False 15 | 16 | def __le__(self, other: object) -> bool: 17 | return False 18 | 19 | def __eq__(self, other: object) -> bool: 20 | return isinstance(other, self.__class__) 21 | 22 | def __gt__(self, other: object) -> bool: 23 | return True 24 | 25 | def __ge__(self, other: object) -> bool: 26 | return True 27 | 28 | def __neg__(self: object) -> "NegativeInfinityType": 29 | return NegativeInfinity 30 | 31 | 32 | Infinity = InfinityType() 33 | 34 | 35 | class NegativeInfinityType: 36 | def __repr__(self) -> str: 37 | return "-Infinity" 38 | 39 | def __hash__(self) -> int: 40 | return hash(repr(self)) 41 | 42 | def __lt__(self, other: object) -> bool: 43 | return True 44 | 45 | def __le__(self, other: object) -> bool: 46 | return True 47 | 48 | def __eq__(self, other: object) -> bool: 49 | return isinstance(other, self.__class__) 50 | 51 | def __gt__(self, other: object) -> bool: 52 | return False 53 | 54 | def __ge__(self, other: object) -> bool: 55 | return False 56 | 57 | def __neg__(self: object) -> InfinityType: 58 | return Infinity 59 | 60 | 61 | NegativeInfinity = NegativeInfinityType() 62 | -------------------------------------------------------------------------------- /tools/msg/tools/packaging/py.typed: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shawnfeng0/uorb/c6f042ec210d02aa71c6241375d79c1775622d51/tools/msg/tools/packaging/py.typed -------------------------------------------------------------------------------- /tools/msg/tools/packaging/requirements.py: -------------------------------------------------------------------------------- 1 | # This file is dual licensed under the terms of the Apache License, Version 2 | # 2.0, and the BSD License. See the LICENSE file in the root of this repository 3 | # for complete details. 4 | 5 | import re 6 | import string 7 | import urllib.parse 8 | from typing import List, Optional as TOptional, Set 9 | 10 | from pyparsing import ( # noqa 11 | Combine, 12 | Literal as L, 13 | Optional, 14 | ParseException, 15 | Regex, 16 | Word, 17 | ZeroOrMore, 18 | originalTextFor, 19 | stringEnd, 20 | stringStart, 21 | ) 22 | 23 | from .markers import MARKER_EXPR, Marker 24 | from .specifiers import LegacySpecifier, Specifier, SpecifierSet 25 | 26 | 27 | class InvalidRequirement(ValueError): 28 | """ 29 | An invalid requirement was found, users should refer to PEP 508. 30 | """ 31 | 32 | 33 | ALPHANUM = Word(string.ascii_letters + string.digits) 34 | 35 | LBRACKET = L("[").suppress() 36 | RBRACKET = L("]").suppress() 37 | LPAREN = L("(").suppress() 38 | RPAREN = L(")").suppress() 39 | COMMA = L(",").suppress() 40 | SEMICOLON = L(";").suppress() 41 | AT = L("@").suppress() 42 | 43 | PUNCTUATION = Word("-_.") 44 | IDENTIFIER_END = ALPHANUM | (ZeroOrMore(PUNCTUATION) + ALPHANUM) 45 | IDENTIFIER = Combine(ALPHANUM + ZeroOrMore(IDENTIFIER_END)) 46 | 47 | NAME = IDENTIFIER("name") 48 | EXTRA = IDENTIFIER 49 | 50 | URI = Regex(r"[^ ]+")("url") 51 | URL = AT + URI 52 | 53 | EXTRAS_LIST = EXTRA + ZeroOrMore(COMMA + EXTRA) 54 | EXTRAS = (LBRACKET + Optional(EXTRAS_LIST) + RBRACKET)("extras") 55 | 56 | VERSION_PEP440 = Regex(Specifier._regex_str, re.VERBOSE | re.IGNORECASE) 57 | VERSION_LEGACY = Regex(LegacySpecifier._regex_str, re.VERBOSE | re.IGNORECASE) 58 | 59 | VERSION_ONE = VERSION_PEP440 ^ VERSION_LEGACY 60 | VERSION_MANY = Combine( 61 | VERSION_ONE + ZeroOrMore(COMMA + VERSION_ONE), joinString=",", adjacent=False 62 | )("_raw_spec") 63 | _VERSION_SPEC = Optional((LPAREN + VERSION_MANY + RPAREN) | VERSION_MANY) 64 | _VERSION_SPEC.setParseAction(lambda s, l, t: t._raw_spec or "") 65 | 66 | VERSION_SPEC = originalTextFor(_VERSION_SPEC)("specifier") 67 | VERSION_SPEC.setParseAction(lambda s, l, t: t[1]) 68 | 69 | MARKER_EXPR = originalTextFor(MARKER_EXPR())("marker") 70 | MARKER_EXPR.setParseAction( 71 | lambda s, l, t: Marker(s[t._original_start : t._original_end]) 72 | ) 73 | MARKER_SEPARATOR = SEMICOLON 74 | MARKER = MARKER_SEPARATOR + MARKER_EXPR 75 | 76 | VERSION_AND_MARKER = VERSION_SPEC + Optional(MARKER) 77 | URL_AND_MARKER = URL + Optional(MARKER) 78 | 79 | NAMED_REQUIREMENT = NAME + Optional(EXTRAS) + (URL_AND_MARKER | VERSION_AND_MARKER) 80 | 81 | REQUIREMENT = stringStart + NAMED_REQUIREMENT + stringEnd 82 | # pyparsing isn't thread safe during initialization, so we do it eagerly, see 83 | # issue #104 84 | REQUIREMENT.parseString("x[]") 85 | 86 | 87 | class Requirement: 88 | """Parse a requirement. 89 | 90 | Parse a given requirement string into its parts, such as name, specifier, 91 | URL, and extras. Raises InvalidRequirement on a badly-formed requirement 92 | string. 93 | """ 94 | 95 | # TODO: Can we test whether something is contained within a requirement? 96 | # If so how do we do that? Do we need to test against the _name_ of 97 | # the thing as well as the version? What about the markers? 98 | # TODO: Can we normalize the name and extra name? 99 | 100 | def __init__(self, requirement_string: str) -> None: 101 | try: 102 | req = REQUIREMENT.parseString(requirement_string) 103 | except ParseException as e: 104 | raise InvalidRequirement( 105 | f'Parse error at "{ requirement_string[e.loc : e.loc + 8]!r}": {e.msg}' 106 | ) 107 | 108 | self.name: str = req.name 109 | if req.url: 110 | parsed_url = urllib.parse.urlparse(req.url) 111 | if parsed_url.scheme == "file": 112 | if urllib.parse.urlunparse(parsed_url) != req.url: 113 | raise InvalidRequirement("Invalid URL given") 114 | elif not (parsed_url.scheme and parsed_url.netloc) or ( 115 | not parsed_url.scheme and not parsed_url.netloc 116 | ): 117 | raise InvalidRequirement(f"Invalid URL: {req.url}") 118 | self.url: TOptional[str] = req.url 119 | else: 120 | self.url = None 121 | self.extras: Set[str] = set(req.extras.asList() if req.extras else []) 122 | self.specifier: SpecifierSet = SpecifierSet(req.specifier) 123 | self.marker: TOptional[Marker] = req.marker if req.marker else None 124 | 125 | def __str__(self) -> str: 126 | parts: List[str] = [self.name] 127 | 128 | if self.extras: 129 | formatted_extras = ",".join(sorted(self.extras)) 130 | parts.append(f"[{formatted_extras}]") 131 | 132 | if self.specifier: 133 | parts.append(str(self.specifier)) 134 | 135 | if self.url: 136 | parts.append(f"@ {self.url}") 137 | if self.marker: 138 | parts.append(" ") 139 | 140 | if self.marker: 141 | parts.append(f"; {self.marker}") 142 | 143 | return "".join(parts) 144 | 145 | def __repr__(self) -> str: 146 | return f"" 147 | -------------------------------------------------------------------------------- /tools/msg/tools/packaging/utils.py: -------------------------------------------------------------------------------- 1 | # This file is dual licensed under the terms of the Apache License, Version 2 | # 2.0, and the BSD License. See the LICENSE file in the root of this repository 3 | # for complete details. 4 | 5 | import re 6 | from typing import FrozenSet, NewType, Tuple, Union, cast 7 | 8 | from .tags import Tag, parse_tag 9 | from .version import InvalidVersion, Version 10 | 11 | BuildTag = Union[Tuple[()], Tuple[int, str]] 12 | NormalizedName = NewType("NormalizedName", str) 13 | 14 | 15 | class InvalidWheelFilename(ValueError): 16 | """ 17 | An invalid wheel filename was found, users should refer to PEP 427. 18 | """ 19 | 20 | 21 | class InvalidSdistFilename(ValueError): 22 | """ 23 | An invalid sdist filename was found, users should refer to the packaging user guide. 24 | """ 25 | 26 | 27 | _canonicalize_regex = re.compile(r"[-_.]+") 28 | # PEP 427: The build number must start with a digit. 29 | _build_tag_regex = re.compile(r"(\d+)(.*)") 30 | 31 | 32 | def canonicalize_name(name: str) -> NormalizedName: 33 | # This is taken from PEP 503. 34 | value = _canonicalize_regex.sub("-", name).lower() 35 | return cast(NormalizedName, value) 36 | 37 | 38 | def canonicalize_version(version: Union[Version, str]) -> str: 39 | """ 40 | This is very similar to Version.__str__, but has one subtle difference 41 | with the way it handles the release segment. 42 | """ 43 | if isinstance(version, str): 44 | try: 45 | parsed = Version(version) 46 | except InvalidVersion: 47 | # Legacy versions cannot be normalized 48 | return version 49 | else: 50 | parsed = version 51 | 52 | parts = [] 53 | 54 | # Epoch 55 | if parsed.epoch != 0: 56 | parts.append(f"{parsed.epoch}!") 57 | 58 | # Release segment 59 | # NB: This strips trailing '.0's to normalize 60 | parts.append(re.sub(r"(\.0)+$", "", ".".join(str(x) for x in parsed.release))) 61 | 62 | # Pre-release 63 | if parsed.pre is not None: 64 | parts.append("".join(str(x) for x in parsed.pre)) 65 | 66 | # Post-release 67 | if parsed.post is not None: 68 | parts.append(f".post{parsed.post}") 69 | 70 | # Development release 71 | if parsed.dev is not None: 72 | parts.append(f".dev{parsed.dev}") 73 | 74 | # Local version segment 75 | if parsed.local is not None: 76 | parts.append(f"+{parsed.local}") 77 | 78 | return "".join(parts) 79 | 80 | 81 | def parse_wheel_filename( 82 | filename: str, 83 | ) -> Tuple[NormalizedName, Version, BuildTag, FrozenSet[Tag]]: 84 | if not filename.endswith(".whl"): 85 | raise InvalidWheelFilename( 86 | f"Invalid wheel filename (extension must be '.whl'): {filename}" 87 | ) 88 | 89 | filename = filename[:-4] 90 | dashes = filename.count("-") 91 | if dashes not in (4, 5): 92 | raise InvalidWheelFilename( 93 | f"Invalid wheel filename (wrong number of parts): {filename}" 94 | ) 95 | 96 | parts = filename.split("-", dashes - 2) 97 | name_part = parts[0] 98 | # See PEP 427 for the rules on escaping the project name 99 | if "__" in name_part or re.match(r"^[\w\d._]*$", name_part, re.UNICODE) is None: 100 | raise InvalidWheelFilename(f"Invalid project name: {filename}") 101 | name = canonicalize_name(name_part) 102 | version = Version(parts[1]) 103 | if dashes == 5: 104 | build_part = parts[2] 105 | build_match = _build_tag_regex.match(build_part) 106 | if build_match is None: 107 | raise InvalidWheelFilename( 108 | f"Invalid build number: {build_part} in '{filename}'" 109 | ) 110 | build = cast(BuildTag, (int(build_match.group(1)), build_match.group(2))) 111 | else: 112 | build = () 113 | tags = parse_tag(parts[-1]) 114 | return (name, version, build, tags) 115 | 116 | 117 | def parse_sdist_filename(filename: str) -> Tuple[NormalizedName, Version]: 118 | if filename.endswith(".tar.gz"): 119 | file_stem = filename[: -len(".tar.gz")] 120 | elif filename.endswith(".zip"): 121 | file_stem = filename[: -len(".zip")] 122 | else: 123 | raise InvalidSdistFilename( 124 | f"Invalid sdist filename (extension must be '.tar.gz' or '.zip'):" 125 | f" {filename}" 126 | ) 127 | 128 | # We are requiring a PEP 440 version, which cannot contain dashes, 129 | # so we split on the last dash. 130 | name_part, sep, version_part = file_stem.rpartition("-") 131 | if not sep: 132 | raise InvalidSdistFilename(f"Invalid sdist filename: {filename}") 133 | 134 | name = canonicalize_name(name_part) 135 | version = Version(version_part) 136 | return (name, version) 137 | -------------------------------------------------------------------------------- /tools/msg/tools/requirements.txt: -------------------------------------------------------------------------------- 1 | empy 2 | pyros-genmsg 3 | packaging -------------------------------------------------------------------------------- /tools/uorb_tcp_topic_listener_lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(uorb_tcp_topic_listener_lib) 2 | 3 | add_library(${PROJECT_NAME} src/uorb_tcp_listener.cc) 4 | target_include_directories(${PROJECT_NAME} PUBLIC include) 5 | target_link_libraries(${PROJECT_NAME} PRIVATE uorb) 6 | -------------------------------------------------------------------------------- /tools/uorb_tcp_topic_listener_lib/include/uorb_tcp_listener.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 2021/9/30. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | /** 11 | * @brief Callback function to return theme metadata 12 | */ 13 | typedef const struct orb_metadata *const *(*orb_get_topics_callback)( 14 | size_t *size); 15 | 16 | #ifdef __cplusplus 17 | extern "C" { 18 | #endif 19 | 20 | /** 21 | * Start a tcp shell for the uorb topic 22 | * 23 | * @param callback @see orb_get_topics_callback 24 | * @param port tcp listener port 25 | * 26 | * Use the following script to connect: 27 | * stty -echo -icanon && nc localhost 10924 28 | */ 29 | void orb_tcp_listener_init(orb_get_topics_callback callback, uint16_t port); 30 | 31 | #ifdef __cplusplus 32 | } 33 | #endif 34 | -------------------------------------------------------------------------------- /tools/uorb_tcp_topic_listener_lib/src/command_manager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 2021/9/27. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "fd_stream.h" 14 | 15 | namespace uorb { 16 | namespace listener { 17 | 18 | class CommandManager { 19 | public: 20 | CommandManager() { 21 | command_map_["help"].entry = [&](uorb::listener::Fd& fd, 22 | const std::vector& args) { 23 | ExecuteHelp(fd, args); 24 | }; 25 | command_map_["help"].comment = "Print command list"; 26 | } 27 | using CommandFunction = std::function& args)>; 29 | 30 | auto AddCommand(const std::string& command, CommandFunction function, 31 | const std::string& comment = "") -> decltype(*this) { 32 | command_map_[command].entry = std::move(function); 33 | command_map_[command].comment = comment; 34 | return *this; 35 | } 36 | 37 | void ExecuteHelp(uorb::listener::Fd& fd, 38 | const std::vector&) const { 39 | fd.write("Command list: \n"); 40 | for (const auto& i : command_map_) { 41 | fd.write("\t" + i.first + ": " + i.second.comment + "\n"); 42 | } 43 | } 44 | 45 | bool ExecuteCommand(const std::string& command, uorb::listener::Fd& fd, 46 | const std::vector& args) const { 47 | if (!command_map_.count(command)) { 48 | return false; 49 | } 50 | command_map_.at(command).entry(fd, args); 51 | return true; 52 | } 53 | 54 | private: 55 | struct CommandInfo { 56 | std::string comment; 57 | std::function& args)> 59 | entry; 60 | }; 61 | std::map command_map_; 62 | }; 63 | 64 | } // namespace listener 65 | } // namespace uorb 66 | -------------------------------------------------------------------------------- /tools/uorb_tcp_topic_listener_lib/src/data_printer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 2021/10/4. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | #include 10 | 11 | #include "var_attr.h" 12 | 13 | namespace uorb { 14 | namespace listener { 15 | 16 | class DataPrinter { 17 | public: 18 | explicit DataPrinter(const orb_metadata &meta) 19 | : vars(uorb::listener::ParseMetaFields(meta)) { 20 | size_t size = 0; 21 | for (const auto &v : vars) { 22 | size += v.PrintToStringWithName(nullptr, nullptr); 23 | } 24 | size_ = size; 25 | } 26 | 27 | std::string Convert2String(void *data, size_t size) { 28 | if (size != size_) return "Error: Data length error!"; 29 | std::string result; 30 | auto ptr = reinterpret_cast(data); 31 | for (const auto &v : vars) { 32 | std::string value_string; 33 | ptr += v.PrintToStringWithName(ptr, &value_string); 34 | result += value_string + "\n"; 35 | } 36 | return result; 37 | } 38 | 39 | private: 40 | const std::vector vars; 41 | size_t size_; 42 | }; 43 | 44 | } // namespace listener 45 | } // namespace uorb 46 | -------------------------------------------------------------------------------- /tools/uorb_tcp_topic_listener_lib/src/fd_stream.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 2021/9/27. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace uorb { 16 | namespace listener { 17 | 18 | class Fd { 19 | public: 20 | explicit Fd(int fd) : fd_(fd) { mark_non_block(fd_); } 21 | ~Fd() { ::close(fd_); } 22 | 23 | ssize_t read(void *buf, size_t nbytes) const { 24 | return ::read(fd_, buf, nbytes); 25 | } 26 | 27 | ssize_t write(const void *buf, size_t n) const { 28 | return ::write(fd_, buf, n); 29 | } 30 | 31 | ssize_t write(const std::string &data) const { 32 | return ::write(fd_, data.data(), data.size()); 33 | } 34 | 35 | int poll_in(int timeout_ms) const { 36 | pollfd fd{}; 37 | fd.fd = fd_; 38 | fd.events = POLLIN; 39 | return poll(&fd, 1, timeout_ms); 40 | } 41 | 42 | int get_fd() const { return fd_; } 43 | 44 | Fd(const Fd &) = delete; 45 | Fd &operator=(const Fd &) = delete; 46 | 47 | private: 48 | static void mark_non_block(int fd) { 49 | auto ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL) | O_NONBLOCK); 50 | assert(-1 != ret); 51 | } 52 | 53 | int fd_; 54 | }; 55 | 56 | } // namespace listener 57 | } // namespace uorb 58 | -------------------------------------------------------------------------------- /tools/uorb_tcp_topic_listener_lib/src/string_helper.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 2021/9/27. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | namespace uorb { 11 | namespace listener { 12 | 13 | static constexpr char kDefaultStripChars[] = " \t\r\n"; 14 | 15 | /** 16 | * Strip function, delete the specified characters on both sides of the string. 17 | */ 18 | 19 | inline std::string &left_strip( 20 | std::string &s, const std::string &characters = kDefaultStripChars) { 21 | return s.erase(0, s.find_first_not_of(characters)); 22 | } 23 | 24 | inline std::string left_strip( 25 | const std::string &s, const std::string &characters = kDefaultStripChars) { 26 | return std::string{s}.erase(0, s.find_first_not_of(characters)); 27 | } 28 | 29 | inline std::string &right_strip( 30 | std::string &s, const std::string &characters = kDefaultStripChars) { 31 | return s.erase(s.find_last_not_of(characters) + 1); 32 | } 33 | 34 | inline std::string right_strip( 35 | const std::string &s, const std::string &characters = kDefaultStripChars) { 36 | return std::string{s}.erase(s.find_last_not_of(characters) + 1); 37 | } 38 | 39 | template 40 | inline std::string strip(StrType &&s, 41 | const std::string &characters = kDefaultStripChars) { 42 | return right_strip(left_strip(std::forward(s), characters), 43 | characters); 44 | } 45 | 46 | /** 47 | * Split string to string vector 48 | * 49 | * @code Test: 50 | * std::string str{"program arg1 arg2 \t arg3 \n"}; 51 | * auto str_vec = split_string(str); 52 | * EXPECT_EQ(str_vec.size(), 5); 53 | * EXPECT_EQ(str_vec[0], "program"); 54 | * EXPECT_EQ(str_vec[1], "arg1"); 55 | * EXPECT_EQ(str_vec[2], "arg2"); 56 | * EXPECT_EQ(str_vec[3], "arg3"); 57 | * EXPECT_EQ(str_vec[4], "\n"); 58 | * 59 | * std::string str; 60 | * auto str_vec = split_string(str); 61 | * EXPECT_EQ(str_vec.size(), 0); 62 | * 63 | * std::string str{"a"}; 64 | * auto str_vec = split_string(str); 65 | * EXPECT_EQ(str_vec.size(), 1); 66 | * EXPECT_EQ(str_vec[0], "a"); 67 | * 68 | * std::string str{"a \t"}; 69 | * auto str_vec = split_string(str); 70 | * EXPECT_EQ(str_vec.size(), 1); 71 | * EXPECT_EQ(str_vec[0], "a"); 72 | * @endcode 73 | * 74 | * @param str 75 | * @param delimiter 76 | * @return 77 | */ 78 | inline std::vector split_string( 79 | const std::string &str, const std::string &delimiter = " \t") { 80 | std::vector result; 81 | 82 | std::string::size_type start; 83 | std::string::size_type end = -1; 84 | while (true) { 85 | start = str.find_first_not_of(delimiter, end + 1); 86 | if (start == std::string::npos) break; // over 87 | 88 | end = str.find_first_of(delimiter, start + 1); 89 | 90 | if (end == std::string::npos) { 91 | result.push_back(str.substr(start)); 92 | break; 93 | } 94 | result.push_back(str.substr(start, end - start)); 95 | } 96 | return result; 97 | } 98 | 99 | /** 100 | * Get string line from string 101 | * 102 | * @code 103 | * std::string str("123\n456\n2\n3\n"); 104 | * auto line = get_line(&str); 105 | * EXPECT_EQ(line, "123\n"); 106 | * EXPECT_EQ(str, "456\n2\n3\n"); 107 | * 108 | * std::string str("\n2\n3\n"); 109 | * auto line = get_line(&str); 110 | * EXPECT_EQ(line, "\n"); 111 | * EXPECT_EQ(str, "2\n3\n"); 112 | * 113 | * std::string str("\n"); 114 | * auto line = get_line(&str); 115 | * EXPECT_EQ(line, "\n"); 116 | * EXPECT_EQ(str, ""); 117 | * @endcode 118 | * 119 | * @param str 120 | * @return one line string 121 | * 122 | */ 123 | std::string get_line(std::string *str) { 124 | if (!str) return ""; 125 | std::string &str_raw = *str; 126 | std::string line; 127 | auto pos = str_raw.find_first_of('\n'); 128 | if (pos != std::string::npos) { 129 | line = str_raw.substr(0, pos + 1); 130 | str_raw.erase(0, pos + 1); 131 | } 132 | return line; 133 | } 134 | 135 | } // namespace listener 136 | } // namespace uorb 137 | -------------------------------------------------------------------------------- /tools/uorb_tcp_topic_listener_lib/src/tcp_server.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 2021/9/30. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace uorb { 10 | namespace listener { 11 | 12 | class TcpServer { 13 | public: 14 | explicit TcpServer(uint16_t port) { 15 | // Creating socket file descriptor 16 | if ((server_fd_ = socket(AF_INET, SOCK_STREAM, 0)) == 0) { 17 | perror("socket failed"); 18 | exit(EXIT_FAILURE); 19 | } 20 | 21 | int opt = 1; 22 | // Forcefully attaching socket to the port 23 | if (setsockopt(server_fd_, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, &opt, 24 | sizeof(opt))) { 25 | perror("setsockopt"); 26 | exit(EXIT_FAILURE); 27 | } 28 | 29 | address_.sin_family = AF_INET; 30 | address_.sin_addr.s_addr = INADDR_ANY; 31 | address_.sin_port = htons(port); 32 | 33 | // Forcefully attaching socket to the port 34 | if (bind(server_fd_, (struct sockaddr *)&address_, sizeof(address_)) < 0) { 35 | perror("bind failed"); 36 | exit(EXIT_FAILURE); 37 | } 38 | if (listen(server_fd_, 30) < 0) { 39 | perror("listen"); 40 | exit(EXIT_FAILURE); 41 | } 42 | } 43 | int accept() { 44 | int addrlen = sizeof(address_); 45 | return ::accept(server_fd_, (struct sockaddr *)&address_, 46 | (socklen_t *)&addrlen); 47 | } 48 | 49 | TcpServer(const TcpServer &) = delete; 50 | TcpServer &operator=(const TcpServer &) = delete; 51 | 52 | private: 53 | struct sockaddr_in address_ {}; 54 | int server_fd_; 55 | }; 56 | 57 | } // namespace listener 58 | } // namespace uorb 59 | -------------------------------------------------------------------------------- /tools/uorb_tcp_topic_listener_lib/src/uorb_tcp_listener.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 2021/9/30. 3 | // 4 | 5 | #include "uorb_tcp_listener.h" 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "command_manager.h" 15 | #include "data_printer.h" 16 | #include "fd_stream.h" 17 | #include "tcp_server.h" 18 | 19 | static orb_get_topics_callback global_topics_callback_ = nullptr; 20 | 21 | /* 22 | * Returns array of topics metadata 23 | */ 24 | static const struct orb_metadata *const *orb_get_topics(size_t *size) { 25 | if (global_topics_callback_ != nullptr) { 26 | return global_topics_callback_(size); 27 | } else { 28 | if (size) *size = 0; 29 | return nullptr; 30 | } 31 | } 32 | 33 | static void CmdGetVersion(uorb::listener::Fd &fd, 34 | const std::vector &) { 35 | fd.write(orb_version()); 36 | fd.write("\n"); 37 | } 38 | 39 | static const orb_metadata *find_meta(const std::string &topic_name) { 40 | size_t orb_topics_count = 0; 41 | auto topics = orb_get_topics(&orb_topics_count); 42 | for (size_t i = 0; i < orb_topics_count; ++i) 43 | if (topic_name == topics[i]->o_name) return topics[i]; 44 | return nullptr; 45 | } 46 | 47 | static void CmdListener(uorb::listener::Fd &fd, 48 | const std::vector &argv) { 49 | if (argv.empty()) { 50 | fd.write("Need topic name, example: listener \n"); 51 | return; 52 | } 53 | auto topic_name = argv[0]; 54 | 55 | auto meta = find_meta(topic_name); 56 | if (!meta) { 57 | fd.write("Can't find topic: " + topic_name + "\n"); 58 | return; 59 | } 60 | 61 | auto sub = orb_create_subscription(meta); 62 | std::vector data(meta->o_size); 63 | 64 | orb_pollfd fds{.fd = sub, .events = POLLIN, .revents = 0}; 65 | 66 | uorb::listener::DataPrinter data_printer(*meta); 67 | orb_abstime_us last_write_timestamp{}; 68 | const int timeout_ms = 1000; 69 | uint32_t current_timeout_ms = 0; 70 | do { 71 | if (orb_poll(&fds, 1, timeout_ms) > 0) { 72 | if (orb_check_and_copy(sub, data.data())) { 73 | using namespace uorb::time_literals; 74 | if (orb_elapsed_time_us(last_write_timestamp) > 100_ms) { 75 | last_write_timestamp = orb_absolute_time_us(); 76 | fd.write(data_printer.Convert2String(data.data(), data.size())); 77 | } 78 | } else { 79 | fd.write("Error: Polling was successful but no data was read.\n"); 80 | } 81 | current_timeout_ms = 0; // Data has been obtained, clear the timeout 82 | } else { 83 | current_timeout_ms += timeout_ms; 84 | fd.write(std::to_string(current_timeout_ms) + "ms" + 85 | std::string{" timeout for polling data\n"}); 86 | } 87 | char c; 88 | auto ret = fd.read(&c, 1); 89 | // == 0 means socket is closed; > 0 means need quit the function. 90 | if (ret >= 0) { 91 | break; 92 | } 93 | if (ret == -1 && errno == EAGAIN) { 94 | continue; 95 | } 96 | } while (true); 97 | 98 | orb_destroy_subscription(&sub); 99 | } 100 | 101 | static void CmdStatus(uorb::listener::Fd &fd, 102 | const std::vector &) { 103 | char send_buffer[256]; 104 | snprintf(send_buffer, sizeof(send_buffer), 105 | "%-20s %-10s %-10s %-10s %-10s %-10s\n", "topic", "instance", 106 | "queue", "sub", "pub", "index"); 107 | fd.write(send_buffer); 108 | 109 | size_t orb_topics_count = 0; 110 | auto topics = orb_get_topics(&orb_topics_count); 111 | for (size_t i = 0; i < orb_topics_count; ++i) { 112 | for (size_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; ++instance) { 113 | orb_status status{}; 114 | if (orb_get_topic_status(topics[i], instance, &status)) { 115 | std::string sub_count_str = std::to_string(status.subscriber_count); 116 | if (status.has_anonymous_subscriber) sub_count_str += "+"; 117 | 118 | std::string pub_count_str = std::to_string(status.publisher_count); 119 | if (status.has_anonymous_publisher) pub_count_str += "+"; 120 | 121 | snprintf(send_buffer, sizeof(send_buffer), 122 | "%-20s %-10zu %-10d %-10s %-10s %-10d\n", topics[i]->o_name, 123 | instance, status.queue_size, sub_count_str.c_str(), 124 | pub_count_str.c_str(), status.latest_data_index); 125 | fd.write(send_buffer); 126 | } 127 | } 128 | } 129 | } 130 | 131 | static void TcpSocketSendThread( 132 | int socket_fd, const uorb::listener::CommandManager &command_manager) { 133 | uorb::listener::Fd socket(socket_fd); 134 | 135 | std::string read_buffer; 136 | 137 | while (true) { 138 | if (socket.poll_in(-1) <= 0) { 139 | continue; 140 | } 141 | 142 | char buffer[1024]; 143 | auto n = socket.read(buffer, sizeof(buffer)); 144 | if (n == 0) break; 145 | if (n < 0) continue; 146 | 147 | read_buffer.append(buffer, buffer + n); 148 | auto line = uorb::listener::get_line(&read_buffer); 149 | if (line.empty()) continue; 150 | 151 | auto args = uorb::listener::split_string(line, " \t\n"); 152 | std::string command; 153 | if (!args.empty()) { 154 | command = *args.begin(); 155 | args.erase(args.begin()); 156 | } 157 | uorb::listener::strip(command); 158 | if (!command.empty()) { 159 | if (!command_manager.ExecuteCommand(command, socket, args)) { 160 | socket.write("Command not found: " + command + "\n"); 161 | command_manager.ExecuteHelp(socket, args); 162 | } 163 | break; // Execute the command only once 164 | } 165 | } 166 | } 167 | 168 | static void TcpServerThread(uint16_t port) { 169 | uorb::listener::CommandManager command_manager; 170 | command_manager.AddCommand("version", CmdGetVersion, "Print uorb version"); 171 | command_manager.AddCommand("status", CmdStatus, "Print uorb status"); 172 | command_manager.AddCommand("listener", CmdListener, 173 | "topic listener, example: listener topic_name"); 174 | 175 | // Prevent process shutdown due to SIGPIPE signal: 176 | // https://stackoverflow.com/questions/54871085/is-it-a-good-practice-to-call-pthread-sigmask-in-a-thread-created-by-stdthread 177 | sigset_t mask; 178 | sigemptyset(&mask); 179 | sigaddset(&mask, SIGPIPE); 180 | pthread_sigmask(SIG_BLOCK, &mask, nullptr); 181 | 182 | uorb::listener::TcpServer tcp_server(port); 183 | 184 | int new_socket; 185 | while ((new_socket = tcp_server.accept()) >= 0) { 186 | std::thread{TcpSocketSendThread, new_socket, command_manager}.detach(); 187 | } 188 | printf("TCP server finished"); 189 | } 190 | 191 | void orb_tcp_listener_init(orb_get_topics_callback callback, uint16_t port) { 192 | global_topics_callback_ = callback; 193 | std::thread{TcpServerThread, port}.detach(); 194 | } 195 | -------------------------------------------------------------------------------- /tools/uorb_tcp_topic_listener_lib/src/var_attr.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shawnfeng on 2021/10/3. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "string_helper.h" 10 | 11 | namespace uorb { 12 | namespace listener { 13 | 14 | enum class ValueType : std::uint8_t { 15 | unknown = 0x00, 16 | int8_t_type = 0x10, 17 | uint8_t_type = 0x11, 18 | bool_type = 0x12, 19 | char_type = 0x13, 20 | int16_t_type = 0x20, 21 | uint16_t_type = 0x21, 22 | int32_t_type = 0x40, 23 | uint32_t_type = 0x41, 24 | float_type = 0x42, 25 | int64_t_type = 0x80, 26 | uint64_t_type = 0x81, 27 | double_type = 0x82, 28 | }; 29 | 30 | /** 31 | * VariableAttribute 32 | * Example: "uint16_t[32] name" 33 | */ 34 | class VarAttr { 35 | public: 36 | explicit VarAttr(const std::string &type_and_name) { 37 | static const std::regex var_attr_regex(R"((\w+)(\[(\d+)\])? (\w+))"); 38 | std::smatch matches; 39 | if (regex_search(type_and_name, matches, var_attr_regex)) { 40 | type_name_ = matches[1]; 41 | if (matches[3].matched) { 42 | array_size_ = std::stoi(matches[3]); 43 | } 44 | var_name_ = matches[4]; 45 | } 46 | } 47 | 48 | size_t PrintToStringWithName(void *data, std::string *str) const { 49 | if (!str) { 50 | return PrintToString(data, nullptr); 51 | } 52 | 53 | *str += type_name_; 54 | if (array_size_) *str += "[" + std::to_string(array_size_) + "]"; 55 | *str += " " + var_name_; 56 | 57 | std::string value_str; 58 | auto ret = PrintToString(data, &value_str); 59 | *str += " = " + value_str; 60 | 61 | return ret; 62 | } 63 | 64 | private: 65 | std::string type_name_{}; 66 | std::string var_name_{}; 67 | size_t array_size_{0}; // 0 means not an array 68 | 69 | /** 70 | * Print data to a string according to the attributes. 71 | * @param data 72 | * @param out_str 73 | * @return Data length used 74 | * @return 0 Error 75 | */ 76 | size_t PrintToString(void *data, std::string *out_str) const { 77 | #define AUX(type) \ 78 | if (type_name_ == #type) { \ 79 | if (out_str) *out_str = var_to_string(data, array_size_); \ 80 | return sizeof(type) * std::max(array_size_, size_t(1)); \ 81 | } 82 | AUX(bool) 83 | AUX(char) 84 | AUX(int8_t) 85 | AUX(uint8_t) 86 | AUX(int16_t) 87 | AUX(uint16_t) 88 | AUX(int32_t) 89 | AUX(uint32_t) 90 | AUX(int64_t) 91 | AUX(uint64_t) 92 | AUX(float) 93 | AUX(double) 94 | #undef AUX 95 | return 0; 96 | } 97 | 98 | template 99 | static std::string var_to_string(void *data, size_t array_size) { 100 | // Non-array 101 | if (array_size < 1) { 102 | T value = *reinterpret_cast(data); 103 | return std::to_string(value); 104 | } else { 105 | // array 106 | std::string result; 107 | result += "["; 108 | for (size_t i = 0; i < array_size; ++i) { 109 | T value = *reinterpret_cast(data); 110 | result += std::to_string(value) + (i != array_size - 1 ? ", " : ""); 111 | data = reinterpret_cast(data) + sizeof(T); 112 | } 113 | result += "]"; 114 | return result; 115 | } 116 | } 117 | }; // namespace listener 118 | 119 | static std::vector ParseMetaFields( 120 | const orb_metadata &meta) { 121 | std::vector result; 122 | auto vars = uorb::listener::split_string(meta.o_fields, ";"); 123 | result.reserve(vars.size()); 124 | for (const auto &var : vars) { 125 | result.emplace_back(var); 126 | } 127 | return result; 128 | } 129 | 130 | } // namespace listener 131 | } // namespace uorb 132 | --------------------------------------------------------------------------------