├── .github └── workflows │ ├── lint.yml │ └── main.yml ├── .gitignore ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── docs ├── fixed_size_messages.png └── ros2_loan_messages.png └── rmw_iceoryx_cpp ├── CMakeLists.txt ├── README.md ├── include └── rmw_iceoryx_cpp │ ├── iceoryx_deserialize.hpp │ ├── iceoryx_get_topic_endpoint_info.hpp │ ├── iceoryx_name_conversion.hpp │ ├── iceoryx_serialize.hpp │ ├── iceoryx_topic_names_and_types.hpp │ └── iceoryx_type_info_introspection.hpp ├── package.xml ├── src ├── iceoryx_generate_gid.hpp ├── iceoryx_identifier.hpp ├── iceoryx_serialization_format.hpp ├── iceoryx_types.hpp ├── internal │ ├── iceoryx_deserialize.cpp │ ├── iceoryx_deserialize_typesupport_c.hpp │ ├── iceoryx_deserialize_typesupport_cpp.hpp │ ├── iceoryx_generate_gid.cpp │ ├── iceoryx_get_topic_endpoint_info.cpp │ ├── iceoryx_name_conversion.cpp │ ├── iceoryx_serialization_common.hpp │ ├── iceoryx_serialize.cpp │ ├── iceoryx_serialize_typesupport_c.hpp │ ├── iceoryx_serialize_typesupport_cpp.hpp │ ├── iceoryx_topic_names_and_types.cpp │ └── iceoryx_type_info_introspection.cpp ├── rmw_client.cpp ├── rmw_compare_guids_equal.cpp ├── rmw_count.cpp ├── rmw_dynamic_type_support.cpp ├── rmw_event.cpp ├── rmw_feature.cpp ├── rmw_get_gid_for_publisher.cpp ├── rmw_get_implementation_identifier.cpp ├── rmw_get_serialization_format.cpp ├── rmw_get_topic_endpoint_info.cpp ├── rmw_guard_condition.cpp ├── rmw_init.cpp ├── rmw_logging.cpp ├── rmw_network_flow_endpoint.cpp ├── rmw_node.cpp ├── rmw_node_info_and_types.cpp ├── rmw_node_names.cpp ├── rmw_publish.cpp ├── rmw_publisher.cpp ├── rmw_qos.cpp ├── rmw_request.cpp ├── rmw_response.cpp ├── rmw_serialize.cpp ├── rmw_service.cpp ├── rmw_service_names_and_types.cpp ├── rmw_service_server_is_available.cpp ├── rmw_subscription.cpp ├── rmw_take.cpp ├── rmw_topic_names_and_types.cpp ├── rmw_trigger_guard_condition.cpp ├── rmw_wait.cpp ├── rmw_wait_set.cpp └── types │ ├── iceoryx_client.hpp │ ├── iceoryx_node.hpp │ ├── iceoryx_publisher.hpp │ ├── iceoryx_server.hpp │ └── iceoryx_subscription.hpp └── test ├── iceoryx_fixed_size_messages_test.cpp ├── iceoryx_name_conversion_test.cpp ├── iceoryx_serialization_test.cpp └── test_msgs_c_fixtures.hpp /.github/workflows/lint.yml: -------------------------------------------------------------------------------- 1 | name: Lint rmw_iceoryx 2 | on: 3 | pull_request: 4 | 5 | jobs: 6 | lint: 7 | name: ament_${{ matrix.linter }} 8 | runs-on: ubuntu-22.04 9 | strategy: 10 | fail-fast: false 11 | matrix: 12 | linter: [cpplint, uncrustify, xmllint, copyright] 13 | steps: 14 | - uses: actions/checkout@v2 15 | - uses: ros-tooling/setup-ros@master 16 | - uses: ros-tooling/action-ros-lint@master 17 | with: 18 | linter: ${{ matrix.linter }} 19 | package-name: | 20 | rmw_iceoryx_cpp 21 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: Integration build rmw_iceoryx 2 | 3 | on: 4 | push: 5 | branches: 6 | - iron 7 | pull_request: 8 | 9 | jobs: 10 | build: 11 | runs-on: ${{ matrix.os }} 12 | strategy: 13 | fail-fast: false 14 | matrix: 15 | os: [ubuntu-22.04] 16 | steps: 17 | - uses: actions/checkout@v2 18 | - name: Setup ROS 19 | uses: ros-tooling/setup-ros@master 20 | with: 21 | required-ros-distributions: iron 22 | - name: Install Iceoryx Dependencies 23 | run: sudo apt-get update && sudo apt-get install -y cmake libacl1-dev libncurses5-dev pkg-config 24 | - name: Build & Test 25 | uses: ros-tooling/action-ros-ci@master 26 | with: 27 | package-name: rmw_iceoryx_cpp 28 | target-ros2-distro: iron 29 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | .vscode/ 3 | build/ 4 | install/ 5 | log/ 6 | rclcpp/ 7 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | 15 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 16 | line to commit messages to certify that they have the right to submit 17 | the code they are contributing to the project according to the 18 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 19 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![Integration build and tests](https://github.com/ros2/rmw_iceoryx/workflows/Integration%20build%20rmw_iceoryx/badge.svg) 2 | ![Linting](https://github.com/ros2/rmw_iceoryx/workflows/Lint%20rmw_iceoryx/badge.svg) 3 | ![Iceoryx build and tests](https://github.com/ros2/rmw_iceoryx/workflows/Build%20iceoryx/badge.svg) 4 | 5 | Installation 6 | ============ 7 | 8 | The following instructions show you how to install the iceoryx rmw implementation. 9 | The installation of rmw_iceoryx is pretty straight forward as [iceoryx](https://github.com/eclipse/iceoryx) is available in [ros2.repos](https://github.com/ros2/ros2/blob/master/ros2.repos). 10 | All provided packages can be built with colcon so that you can easily build rmw_iceoryx within your ROS 2 workspace. 11 | rmw_iceoryx is using the [rosidl_typesupport_introspection](https://github.com/ros2/rosidl) which allows for building iceoryx on top of an existing ROS 2 workspace or even debian installation as no ROS 2 messages have to be built again. 12 | 13 | To install rmw_iceoryx in a ROS 2 workspace with the latest ROS version, just execute the steps below: 14 | 15 | ```bash 16 | mkdir -p ~/iceoryx_ws/src 17 | cd $_ 18 | # LATEST_ROS_VERSION could be e.g. iron 19 | git clone --branch LATEST_ROS_VERSION https://github.com/ros2/rmw_iceoryx.git 20 | ``` 21 | 22 | For alternative installation instructions and more details about iceoryx's internals, please see [iceoryx's GitHub repo](https://github.com/eclipse/iceoryx). 23 | 24 | rmw_iceoryx is compatible with ROS 2 starting with Eloquent release. 25 | Assuming you have ROS 2 installed correctly, you can compile the iceoryx workspace with colcon: 26 | 27 | ```bash 28 | cd ~/iceoryx_ws/ 29 | source /opt/ros/LATEST_ROS_VERSION/setup.bash # alternatively source your own ROS 2 workspace 30 | rosdep update 31 | rosdep install --from-paths src --ignore-src --rosdistro LATEST_ROS_VERSION -y 32 | colcon build 33 | # or with more options 34 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF 35 | ``` 36 | 37 | That's it! You've installed iceoryx and are ready to rumble. 38 | 39 | Working with rmw_iceoryx_cpp 40 | ============================ 41 | 42 | Getting Started 43 | --------------- 44 | 45 | iceoryx is based on shared memory and features a shared memory management application called RouDi. 46 | RouDi is a daemon taking care of allocating enough space within the shared memory each node and is responsible for transporting messages between these nodes. 47 | 48 | Before starting any iceoryx application, we therefore have to start the daemon. 49 | 50 | ```bash 51 | ./iceoryx_ws/install/iceoryx_posh/bin/iox-roudi # /iceoryx_ws/install/bin/iox-roudi if you installed with as a merged workspace 52 | ``` 53 | 54 | You can then use rmw_iceoryx_cpp just like any other available rmw implementation. 55 | In order to specify the rmw implementation, you have to set the environment variable `RMW_IMPLEMENTATION` to `rmw_iceoryx_cpp`. 56 | 57 | To run the ROS 2 c++ demo nodes with iceoryx, you can thus execute the following command: 58 | 59 | ```bash 60 | source ~/iceoryx_ws/install/setup.bash 61 | RMW_IMPLEMENTATION=rmw_iceoryx_cpp ros2 run demo_nodes_cpp talker 62 | ``` 63 | 64 | In another terminal, you can then subscribe to the talker as always: 65 | 66 | ```bash 67 | source ~/iceoryx_ws/install/setup.bash 68 | RMW_IMPLEMENTATION=rmw_iceoryx_cpp ros2 run demo_nodes_cpp listener 69 | ``` 70 | 71 | Zero Copy - The True Power 72 | -------------------------- 73 | 74 | To exploit iceoryx's full potential, we want to leverage the zero copy transport mechanism it provides. 75 | For this to work, we have to take one step back and look at the details of what it means to enable zero copy data transport. 76 | 77 | The basic zero copy workflow works as depicted in the picture below: 78 | ![](docs/ros2_loan_messages.png) 79 | 80 | Step 1 `loan_message()`) A publisher asks rmw_iceoryx_cpp to loan a message from it. 81 | rmw_iceoryx_cpp allocates the appropriate message in its shared memory and loans it to the publisher. 82 | 83 | Step 2 `publish()`) The publisher can fill in the data into the loaned message. 84 | When calling publish, the loaned message will be returned to the middleware and the publisher has no longer ownership over the message. 85 | 86 | Step 3 `take_loaned_message()`) A subscription wants to take a message from the middleware. 87 | rmw_iceoryx_cpp gives a loaned message to the subscription which can execute their respective callbacks. 88 | 89 | Step 4 `return_loaned_message()`) A subscription callback is finished and the loaned message is getting returned to the middleware. 90 | 91 | Starting from ROS 2 Eloquent, these features are implemented within rclcpp. 92 | An application using these new features is shown in the code snippet below. 93 | For a fully working implementation, please have a look at [this demo node](https://github.com/ros2/demos/blob/master/demo_nodes_cpp/src/topics/talker_loaned_message.cpp). 94 | 95 | ```c++ 96 | auto non_pod_pub_ = node->create_publisher("/chatter", 1); 97 | // Ask the publisher to loan a String message 98 | auto non_pod_loaned_msg = non_pod_pub_->borrow_loaned_message(); 99 | non_pod_loaned_msg.get().data = "Hello World"; 100 | // Return ownership of that string message 101 | non_pod_pub_->publish(std::move(non_pod_loaned_msg)); 102 | ``` 103 | 104 | The code above has one problem though: How can the middleware allocate enough memory for the string message? 105 | The middleware can't possibly know the size of the string the user wants to put into that message. 106 | 107 | That being said, in order to enable a true zero copy data transport we have to limit ourselves to fixed size data structures. 108 | The picture below tries to illustrate the difference between a fixed size message and a dynamically resizable message. 109 | ![](docs/fixed_size_messages.png) 110 | 111 | The plain old datatype (POD) on the left side is very well suited for zero copy data transport as its size is definitely defined (on compile time). 112 | The message definition shown on the right size is not made for zero copy transport as its size might vary during runtime and rmw_iceoryx_cpp can't determine how much memory should be allocated in the shared memory. 113 | 114 | Thus, in order to make our demo work with zero copy, we can alternatively send a float64, as its size is clearly defined. 115 | 116 | ```c++ 117 | auto pod_pub_ = node->create_publisher("/float", 1); 118 | // Ask the publisher to loan a Float64 message 119 | auto pod_loaned_msg = pod_pub_->borrow_loaned_message(); 120 | pod_loaned_msg.get().data = 123.456f; 121 | // Return ownership of that Float64 message 122 | pod_pub_->publish(std::move(pod_loaned_msg)); 123 | ``` 124 | 125 | If you'd like to play around with the zero copy transport, we recommend to checkout the [fixed size image transport demo](https://github.com/karsten1987/fixed_size_ros2_demo), which illustrates how iceoryx can be used to publish and subscribe up to even 4K images without having to copy them. 126 | 127 | Limitations 128 | =========== 129 | 130 | rmw_iceoryx_cpp is currently under heavy development. 131 | Unfortunately, not all features are yet fully fleshed out. 132 | 133 | | ROS 2 command/feature | Status | 134 | |-----------------------|------------------------------------| 135 | | `ros2 topic list` | :heavy_check_mark: | 136 | | `ros2 topic echo` | :heavy_check_mark: | 137 | | `ros2 topic type` | :heavy_check_mark: | 138 | | `ros2 topic info` | :heavy_check_mark: | 139 | | `ros2 topic hz` | :heavy_check_mark: | 140 | | `ros2 topic bw` | :heavy_check_mark: | 141 | | `ros2 node list` | :heavy_check_mark: | 142 | | `ros2 node info` | :heavy_check_mark: | 143 | | `ros2 interface *` | :heavy_check_mark: | 144 | | `ros2 service *` | :heavy_check_mark: | 145 | | `ros2 param list` | :x: | 146 | | `rqt_graph` | :heavy_check_mark: | 147 | | `rqt_top` | :heavy_check_mark: | 148 | | `rqt_console` | :heavy_check_mark: | 149 | | `rqt_plot` | :heavy_check_mark: | 150 | | `rviz2` | :heavy_check_mark: | 151 | | `ros2 bag` | :grey_question: | 152 | | urdf | :grey_question: | 153 | | tf2 | :grey_question: | 154 | | RMW Pub/Sub Events | :x: | 155 | -------------------------------------------------------------------------------- /docs/fixed_size_messages.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/rmw_iceoryx/1d03b7eea197dd8980989bf89daa4ef37b6be5e8/docs/fixed_size_messages.png -------------------------------------------------------------------------------- /docs/ros2_loan_messages.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/rmw_iceoryx/1d03b7eea197dd8980989bf89daa4ef37b6be5e8/docs/ros2_loan_messages.png -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rmw_iceoryx_cpp) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Set C++ standard in accordance with iceoryx 10 | # https://github.com/eclipse-iceoryx/iceoryx/blob/v2.0.3/iceoryx_hoofs/cmake/IceoryxPlatform.cmake#L26-L38 11 | if(LINUX) 12 | set(CMAKE_CXX_STANDARD 14) 13 | elseif(QNX) 14 | set(CMAKE_CXX_STANDARD 14) 15 | elseif(WIN32) 16 | set(CMAKE_CXX_STANDARD 17) 17 | elseif(APPLE) 18 | set(CMAKE_CXX_STANDARD 17) 19 | elseif(UNIX) 20 | set(CMAKE_CXX_STANDARD 17) 21 | else() 22 | set(CMAKE_CXX_STANDARD 17) 23 | endif() 24 | 25 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 26 | add_compile_options(-Wall -Wextra -Wpedantic) 27 | endif() 28 | 29 | if(CODE_COVERAGE AND 30 | CMAKE_SYSTEM_NAME MATCHES "Linux" AND 31 | CMAKE_C_COMPILER_ID STREQUAL "GNU") 32 | message("GCov code coverage enabled") 33 | set(CMAKE_CXX_FLAGS "--coverage") 34 | set(CMAKE_EXE_LINKER_FLAGS "--coverage") 35 | endif() 36 | 37 | # find dependencies 38 | find_package(ament_cmake REQUIRED) 39 | find_package(iceoryx_posh REQUIRED) 40 | find_package(rcpputils REQUIRED) 41 | find_package(rcutils REQUIRED) 42 | find_package(rmw REQUIRED) 43 | find_package(rosidl_runtime_c REQUIRED) 44 | find_package(rosidl_runtime_cpp REQUIRED) 45 | find_package(rosidl_typesupport_cpp REQUIRED) 46 | find_package(rosidl_typesupport_introspection_c REQUIRED) 47 | find_package(rosidl_typesupport_introspection_cpp REQUIRED) 48 | 49 | add_library(rmw_iceoryx_serialization SHARED 50 | src/internal/iceoryx_deserialize.cpp 51 | src/internal/iceoryx_serialize.cpp 52 | src/internal/iceoryx_type_info_introspection.cpp 53 | ) 54 | target_include_directories(rmw_iceoryx_serialization 55 | PUBLIC include 56 | ) 57 | ament_target_dependencies(rmw_iceoryx_serialization 58 | rosidl_runtime_c 59 | rosidl_runtime_cpp 60 | rosidl_typesupport_introspection_c 61 | rosidl_typesupport_introspection_cpp 62 | ) 63 | add_library(rmw_iceoryx_name_conversion SHARED 64 | src/internal/iceoryx_name_conversion.cpp 65 | src/internal/iceoryx_topic_names_and_types.cpp 66 | src/internal/iceoryx_get_topic_endpoint_info.cpp 67 | ) 68 | target_include_directories(rmw_iceoryx_name_conversion 69 | PUBLIC include 70 | ) 71 | target_link_libraries(rmw_iceoryx_name_conversion 72 | iceoryx_posh::iceoryx_posh 73 | rmw_iceoryx_serialization 74 | ) 75 | ament_target_dependencies(rmw_iceoryx_name_conversion 76 | rcpputils 77 | rcutils 78 | rmw 79 | rosidl_runtime_c 80 | rosidl_runtime_cpp 81 | rosidl_typesupport_introspection_c 82 | rosidl_typesupport_introspection_cpp 83 | ) 84 | add_library(rmw_iceoryx_cpp SHARED 85 | src/internal/iceoryx_generate_gid.cpp 86 | src/rmw_client.cpp 87 | src/rmw_compare_guids_equal.cpp 88 | src/rmw_count.cpp 89 | src/rmw_event.cpp 90 | src/rmw_feature.cpp 91 | src/rmw_get_gid_for_publisher.cpp 92 | src/rmw_get_implementation_identifier.cpp 93 | src/rmw_get_serialization_format.cpp 94 | src/rmw_get_topic_endpoint_info.cpp 95 | src/rmw_guard_condition.cpp 96 | src/rmw_init.cpp 97 | src/rmw_logging.cpp 98 | src/rmw_network_flow_endpoint.cpp 99 | src/rmw_node.cpp 100 | src/rmw_node_info_and_types.cpp 101 | src/rmw_node_names.cpp 102 | src/rmw_publish.cpp 103 | src/rmw_publisher.cpp 104 | src/rmw_qos.cpp 105 | src/rmw_request.cpp 106 | src/rmw_response.cpp 107 | src/rmw_serialize.cpp 108 | src/rmw_service.cpp 109 | src/rmw_service_names_and_types.cpp 110 | src/rmw_service_server_is_available.cpp 111 | src/rmw_subscription.cpp 112 | src/rmw_take.cpp 113 | src/rmw_topic_names_and_types.cpp 114 | src/rmw_trigger_guard_condition.cpp 115 | src/rmw_wait.cpp 116 | src/rmw_wait_set.cpp 117 | src/rmw_dynamic_type_support.cpp 118 | ) 119 | ament_target_dependencies(rmw_iceoryx_cpp 120 | "rcutils" 121 | "rmw" 122 | "rosidl_typesupport_introspection_cpp" 123 | ) 124 | target_include_directories( 125 | rmw_iceoryx_cpp 126 | PUBLIC include 127 | ) 128 | target_link_libraries(rmw_iceoryx_cpp 129 | rmw_iceoryx_serialization 130 | rmw_iceoryx_name_conversion 131 | iceoryx_posh::iceoryx_posh 132 | ) 133 | 134 | register_rmw_implementation( 135 | "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" 136 | "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp" 137 | ) 138 | configure_rmw_library(rmw_iceoryx_cpp) 139 | 140 | install( 141 | DIRECTORY include/ 142 | DESTINATION include 143 | ) 144 | 145 | install( 146 | TARGETS rmw_iceoryx_serialization rmw_iceoryx_name_conversion rmw_iceoryx_cpp 147 | ARCHIVE DESTINATION lib 148 | LIBRARY DESTINATION lib 149 | RUNTIME DESTINATION bin 150 | ) 151 | 152 | if(BUILD_TESTING) 153 | find_package(ament_lint_auto REQUIRED) 154 | ament_lint_auto_find_test_dependencies() 155 | 156 | find_package(ament_cmake_gtest REQUIRED) 157 | find_package(test_msgs REQUIRED) 158 | 159 | ament_add_gtest(test_name_conversion test/iceoryx_name_conversion_test.cpp) 160 | target_link_libraries(test_name_conversion ${PROJECT_NAME}) 161 | 162 | ament_add_gtest(test_message_serialization test/iceoryx_serialization_test.cpp) 163 | target_link_libraries(test_message_serialization ${PROJECT_NAME}) 164 | ament_target_dependencies(test_message_serialization 165 | test_msgs 166 | ) 167 | 168 | ament_add_gtest(test_fixed_size_messages test/iceoryx_fixed_size_messages_test.cpp) 169 | target_link_libraries(test_fixed_size_messages ${PROJECT_NAME}) 170 | ament_target_dependencies(test_fixed_size_messages 171 | test_msgs 172 | ) 173 | endif() 174 | 175 | ament_export_include_directories(include) 176 | ament_export_libraries(rmw_iceoryx_serialization rmw_iceoryx_name_conversion rmw_iceoryx_cpp) 177 | ament_package() 178 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/README.md: -------------------------------------------------------------------------------- 1 | RMW_ICEORYX_CPP 2 | --------------- 3 | 4 | C++ implementation of the rmw iceoryx middleware interface. 5 | 6 | ## test 7 | 8 | ```sh 9 | /usr/local/bin/iox-roudi -l verbose 10 | 11 | export RMW_IMPLEMENTATION=rmw_iceoryx_cpp 12 | ros2 run demo_nodes_cpp talker 13 | ros2 run demo_nodes_cpp listener 14 | iox-introspection-client --all 15 | ``` 16 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef RMW_ICEORYX_CPP__ICEORYX_DESERIALIZE_HPP_ 17 | #define RMW_ICEORYX_CPP__ICEORYX_DESERIALIZE_HPP_ 18 | 19 | struct rosidl_message_type_support_t; 20 | struct rosidl_service_type_support_t; 21 | 22 | namespace rmw_iceoryx_cpp 23 | { 24 | 25 | /// @todo karsten1987: This should be `uint8`, really 26 | void deserialize( 27 | const char * serialized_msg, 28 | const rosidl_message_type_support_t * type_supports, 29 | void * ros_message); 30 | 31 | void deserializeRequest( 32 | const char * serialized_msg, 33 | const rosidl_service_type_support_t * type_supports, 34 | void * ros_message); 35 | 36 | void deserializeResponse( 37 | const char * serialized_msg, 38 | const rosidl_service_type_support_t * type_supports, 39 | void * ros_message); 40 | 41 | } // namespace rmw_iceoryx_cpp 42 | #endif // RMW_ICEORYX_CPP__ICEORYX_DESERIALIZE_HPP_ 43 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_get_topic_endpoint_info.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 by ZhenshengLee. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef RMW_ICEORYX_CPP__ICEORYX_GET_TOPIC_ENDPOINT_INFO_HPP_ 16 | #define RMW_ICEORYX_CPP__ICEORYX_GET_TOPIC_ENDPOINT_INFO_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "rcutils/types.h" 24 | 25 | #include "rmw/names_and_types.h" 26 | #include "rmw/rmw.h" 27 | #include "rmw/topic_endpoint_info_array.h" 28 | 29 | namespace rmw_iceoryx_cpp 30 | { 31 | /** 32 | * \brief Get topics and their publisher nodes 33 | * \return std::map> 34 | */ 35 | std::map> get_publisher_and_nodes(); 36 | 37 | /** 38 | * \brief Get topics and their subscriber nodes 39 | * \return std::map> 40 | */ 41 | std::map> get_subscriber_and_nodes(); 42 | 43 | /** 44 | * \brief Get publisher node names of one topic 45 | * \param topic_name 46 | * \return std::tuple> 47 | */ 48 | std::tuple> get_publisher_end_info_of_topic( 49 | const char * topic_name); 50 | 51 | /** 52 | * \brief Get subscriber node names of one topic 53 | * \param topic_name 54 | * \return std::tuple> 55 | */ 56 | std::tuple> get_subscriber_end_info_of_topic( 57 | const char * topic_name); 58 | 59 | /** 60 | * \brief helper function to fill rmw structs 61 | * \param rmw_topic_endpoint_info_array 62 | * \param iceoryx_topic_endpoint_info_array 63 | * \param allocator 64 | * \return rmw_ret_t 65 | */ 66 | rmw_ret_t 67 | fill_rmw_publisher_end_info( 68 | rmw_topic_endpoint_info_array_t * rmw_topic_endpoint_info_array, 69 | const std::tuple> & iceoryx_topic_endpoint_info_array, 70 | rcutils_allocator_t * allocator); 71 | 72 | /** 73 | * \brief helper function to fill rmw structs 74 | * \param rmw_topic_endpoint_info_array 75 | * \param iceoryx_topic_endpoint_info_array 76 | * \param allocator 77 | * \return rmw_ret_t 78 | */ 79 | rmw_ret_t 80 | fill_rmw_subscriber_end_info( 81 | rmw_topic_endpoint_info_array_t * rmw_topic_endpoint_info_array, 82 | const std::tuple> & iceoryx_topic_endpoint_info_array, 83 | rcutils_allocator_t * allocator); 84 | 85 | } // namespace rmw_iceoryx_cpp 86 | #endif // RMW_ICEORYX_CPP__ICEORYX_GET_TOPIC_ENDPOINT_INFO_HPP_ 87 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 by ZhenshengLee. All rights reserved. 3 | // Copyright (c) 2023 by Apex.AI Inc. All rights reserved. 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | 17 | #ifndef RMW_ICEORYX_CPP__ICEORYX_NAME_CONVERSION_HPP_ 18 | #define RMW_ICEORYX_CPP__ICEORYX_NAME_CONVERSION_HPP_ 19 | 20 | #include 21 | #include 22 | 23 | #include "iceoryx_posh/capro/service_description.hpp" 24 | #include "iceoryx_hoofs/cxx/string.hpp" 25 | 26 | struct rosidl_message_type_support_t; 27 | struct rosidl_service_type_support_t; 28 | 29 | namespace rmw_iceoryx_cpp 30 | { 31 | /// Get the pair of ros node name and namespace from a given iceoryx node full name. 32 | /** 33 | * \brief Get the name n space from node full name object 34 | * \param node_full_name full name of the node 35 | * \return std::tuple 36 | */ 37 | std::tuple 38 | get_name_n_space_from_node_full_name( 39 | const std::string & node_full_name); 40 | 41 | /// Get the pair of ROS topic and type from a given iceoryx service triplet. 42 | /** 43 | * Given a triplet of `service`, `instance`, `event`, convert these to a ROS2 conform 44 | * tuple of `topic` and `type`. 45 | * For a regular ROS2 pair, the event name should be set to "data". 46 | * \param service the iceoryx service description 47 | * \param instance the iceoryx instance description 48 | * \param event the iceoryx event description 49 | * \return tuple of topic and type 50 | */ 51 | std::tuple 52 | get_name_n_type_from_service_description( 53 | const std::string & service, 54 | const std::string & instance, 55 | const std::string & event); 56 | 57 | /// Get the iceoryx service triplet description from a given pair of ROS topic and type. 58 | /** 59 | * Given a pair in the form of topic name and type, generate a iceoryx service description triplet. 60 | * By convention, the iceoryx event field is set to "data". 61 | * \param service the iceoryx service description 62 | * \param instance the iceoryx instance description 63 | * \param event the iceoryx event description 64 | * \return triple of iceoryx `service`, `instance`, `event`. 65 | */ 66 | std::tuple 67 | get_service_description_from_name_n_type( 68 | const std::string & topic_name, 69 | const std::string & type_name); 70 | 71 | iox::capro::ServiceDescription 72 | get_iceoryx_service_description( 73 | const std::string & topic, 74 | const rosidl_message_type_support_t * type_supports); 75 | 76 | iox::capro::ServiceDescription 77 | get_iceoryx_service_description( 78 | const std::string & topic, 79 | const rosidl_service_type_support_t * type_supports); 80 | 81 | } // namespace rmw_iceoryx_cpp 82 | #endif // RMW_ICEORYX_CPP__ICEORYX_NAME_CONVERSION_HPP_ 83 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_serialize.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef RMW_ICEORYX_CPP__ICEORYX_SERIALIZE_HPP_ 17 | #define RMW_ICEORYX_CPP__ICEORYX_SERIALIZE_HPP_ 18 | 19 | #include 20 | 21 | struct rosidl_message_type_support_t; 22 | struct rosidl_service_type_support_t; 23 | 24 | namespace rmw_iceoryx_cpp 25 | { 26 | 27 | void serialize( 28 | const void * ros_message, 29 | const rosidl_message_type_support_t * type_supports, 30 | std::vector & payload_vector); 31 | 32 | void serializeRequest( 33 | const void * ros_message, 34 | const rosidl_service_type_support_t * type_supports, 35 | std::vector & payload_vector); 36 | 37 | void serializeResponse( 38 | const void * ros_message, 39 | const rosidl_service_type_support_t * type_supports, 40 | std::vector & payload_vector); 41 | 42 | } // namespace rmw_iceoryx_cpp 43 | #endif // RMW_ICEORYX_CPP__ICEORYX_SERIALIZE_HPP_ 44 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef RMW_ICEORYX_CPP__ICEORYX_TOPIC_NAMES_AND_TYPES_HPP_ 17 | #define RMW_ICEORYX_CPP__ICEORYX_TOPIC_NAMES_AND_TYPES_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "rcutils/types.h" 24 | 25 | #include "rmw/names_and_types.h" 26 | #include "rmw/rmw.h" 27 | 28 | namespace rmw_iceoryx_cpp 29 | { 30 | 31 | void fill_topic_containers( 32 | std::map & names_n_types_, 33 | std::map> & subscribers_topics_, 34 | std::map> & publishers_topics_, 35 | std::map> & topic_subscribers_, 36 | std::map> & topic_publishers_); 37 | 38 | std::map get_topic_names_and_types(); 39 | std::map get_service_names_and_types(); 40 | 41 | std::map> get_nodes_and_publishers(); 42 | 43 | std::map> get_nodes_and_subscribers(); 44 | 45 | std::map get_publisher_names_and_types_of_node( 46 | const char * node_name, 47 | const char * node_namespace); 48 | 49 | std::map get_subscription_names_and_types_of_node( 50 | const char * node_name, 51 | const char * node_namespace); 52 | 53 | rmw_ret_t fill_rmw_names_and_types( 54 | rmw_names_and_types_t * rmw_topic_names_and_types, 55 | const std::map & iceoryx_topic_names_and_types, 56 | rcutils_allocator_t * allocator); 57 | 58 | } // namespace rmw_iceoryx_cpp 59 | #endif // RMW_ICEORYX_CPP__ICEORYX_TOPIC_NAMES_AND_TYPES_HPP_ 60 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 - 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef RMW_ICEORYX_CPP__ICEORYX_TYPE_INFO_INTROSPECTION_HPP_ 17 | #define RMW_ICEORYX_CPP__ICEORYX_TYPE_INFO_INTROSPECTION_HPP_ 18 | 19 | #include 20 | #include 21 | 22 | struct rosidl_message_type_support_t; 23 | struct rosidl_service_type_support_t; 24 | 25 | namespace rmw_iceoryx_cpp 26 | { 27 | 28 | enum class TypeSupportLanguage 29 | { 30 | CPP, 31 | C 32 | }; 33 | 34 | /// @brief Wraps get_message_typesupport_handle() and does error handling 35 | /// @return std::pair containing enum TypeSupportLanguage and handle to the type support 36 | const std::pair get_type_support( 37 | const rosidl_message_type_support_t * type_supports); 38 | 39 | /// @brief Wraps get_service_typesupport_handle() and does error handling 40 | /// @return std::pair containing enum TypeSupportLanguage and handle to the type support 41 | const std::pair get_type_support( 42 | const rosidl_service_type_support_t * type_supports); 43 | 44 | bool iceoryx_is_fixed_size(const rosidl_message_type_support_t * type_supports); 45 | bool iceoryx_is_fixed_size(const rosidl_service_type_support_t * type_supports); 46 | 47 | bool iceoryx_is_valid_type_support(const rosidl_message_type_support_t * type_supports); 48 | bool iceoryx_is_valid_type_support(const rosidl_service_type_support_t * type_supports); 49 | 50 | size_t iceoryx_get_message_size(const rosidl_message_type_support_t * type_supports); 51 | size_t iceoryx_get_request_size(const rosidl_service_type_support_t * type_supports); 52 | size_t iceoryx_get_response_size(const rosidl_service_type_support_t * type_supports); 53 | 54 | std::string iceoryx_get_message_name(const rosidl_message_type_support_t * type_supports); 55 | std::string iceoryx_get_service_name(const rosidl_service_type_support_t * type_supports); 56 | 57 | std::string iceoryx_get_message_namespace(const rosidl_message_type_support_t * type_supports); 58 | std::string iceoryx_get_service_namespace(const rosidl_service_type_support_t * type_supports); 59 | 60 | void iceoryx_init_message( 61 | const rosidl_message_type_support_t * type_supports, 62 | void * message); 63 | 64 | void iceoryx_fini_message( 65 | const rosidl_message_type_support_t * type_supports, 66 | void * message); 67 | 68 | } // namespace rmw_iceoryx_cpp 69 | #endif // RMW_ICEORYX_CPP__ICEORYX_TYPE_INFO_INTROSPECTION_HPP_ 70 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rmw_iceoryx_cpp 5 | 2.0.3 6 | rmw implementation for Bosch's zero copy middleware iceoryx 7 | Simon Hoinkis 8 | Apache License 2.0 9 | Karsten Knese 10 | 11 | ament_cmake 12 | 13 | iceoryx_posh 14 | rcpputils 15 | rcutils 16 | rmw 17 | rmw_implementation 18 | rosidl_runtime_c 19 | rosidl_runtime_cpp 20 | rosidl_typesupport_cpp 21 | rosidl_typesupport_introspection_c 22 | rosidl_typesupport_introspection_cpp 23 | 24 | ament_cmake_gtest 25 | ament_lint_auto 26 | ament_lint_common 27 | test_msgs 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/iceoryx_generate_gid.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2022 - 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef ICEORYX_GENERATE_GID_HPP_ 17 | #define ICEORYX_GENERATE_GID_HPP_ 18 | 19 | #include "rmw/types.h" 20 | 21 | #include "iceoryx_posh/popo/untyped_publisher.hpp" 22 | #include "iceoryx_posh/popo/untyped_client.hpp" 23 | 24 | rmw_gid_t generate_publisher_gid(iox::popo::UntypedPublisher * const publisher); 25 | rmw_gid_t generate_client_gid(iox::popo::UntypedClient * const client); 26 | 27 | #endif // ICEORYX_GENERATE_GID_HPP_ 28 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/iceoryx_identifier.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ICEORYX_IDENTIFIER_HPP_ 16 | #define ICEORYX_IDENTIFIER_HPP_ 17 | 18 | extern "C" 19 | { 20 | extern const char * const rmw_iceoryx_cpp_identifier; 21 | } // extern "C" 22 | #endif // ICEORYX_IDENTIFIER_HPP_ 23 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/iceoryx_serialization_format.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ICEORYX_SERIALIZATION_FORMAT_HPP_ 16 | #define ICEORYX_SERIALIZATION_FORMAT_HPP_ 17 | 18 | extern "C" 19 | { 20 | extern const char * const iceoryx_serialization_format; 21 | } // extern "C" 22 | #endif // ICEORYX_SERIALIZATION_FORMAT_HPP_ 23 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/iceoryx_types.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ICEORYX_TYPES_HPP_ 16 | #define ICEORYX_TYPES_HPP_ 17 | 18 | #include "types/iceoryx_guard_condition.hpp" 19 | #include "types/iceoryx_node.hpp" 20 | #include "types/iceoryx_publisher.hpp" 21 | #include "types/iceoryx_subscription.hpp" 22 | 23 | #endif // ICEORYX_TYPES_HPP_ 24 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 - 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "rosidl_typesupport_cpp/message_type_support.hpp" 17 | 18 | #include "rosidl_typesupport_introspection_c/identifier.h" 19 | #include "rosidl_typesupport_introspection_c/message_introspection.h" 20 | #include "rosidl_typesupport_introspection_c/service_introspection.h" 21 | 22 | #include "rosidl_typesupport_introspection_cpp/identifier.hpp" 23 | #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" 24 | 25 | #include "rcutils/error_handling.h" 26 | 27 | #include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" 28 | #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" 29 | 30 | #include "./iceoryx_deserialize_typesupport_c.hpp" 31 | #include "./iceoryx_deserialize_typesupport_cpp.hpp" 32 | 33 | namespace rmw_iceoryx_cpp 34 | { 35 | 36 | void deserialize( 37 | const char * serialized_msg, 38 | const rosidl_message_type_support_t * type_supports, 39 | void * ros_message) 40 | { 41 | auto ts = get_type_support(type_supports); 42 | 43 | if (ts.first == TypeSupportLanguage::CPP) { 44 | auto members_cpp = 45 | static_cast(ts.second->data); 46 | rmw_iceoryx_cpp::details_cpp::deserialize(serialized_msg, members_cpp, ros_message); 47 | } else if (ts.first == TypeSupportLanguage::C) { 48 | auto members_c = 49 | static_cast(ts.second->data); 50 | rmw_iceoryx_cpp::details_c::deserialize(serialized_msg, members_c, ros_message); 51 | } 52 | } 53 | 54 | void deserializeRequest( 55 | const char * serialized_msg, 56 | const rosidl_service_type_support_t * type_supports, 57 | void * ros_message) 58 | { 59 | auto ts = get_type_support(type_supports); 60 | 61 | if (ts.first == TypeSupportLanguage::CPP) { 62 | auto members_cpp = 63 | static_cast(ts.second->data); 64 | rmw_iceoryx_cpp::details_cpp::deserializeRequest(serialized_msg, members_cpp, ros_message); 65 | } else if (ts.first == TypeSupportLanguage::C) { 66 | auto members_c = 67 | static_cast(ts.second->data); 68 | rmw_iceoryx_cpp::details_c::deserializeRequest(serialized_msg, members_c, ros_message); 69 | } 70 | } 71 | 72 | void deserializeResponse( 73 | const char * serialized_msg, 74 | const rosidl_service_type_support_t * type_supports, 75 | void * ros_message) 76 | { 77 | auto ts = get_type_support(type_supports); 78 | 79 | if (ts.first == TypeSupportLanguage::CPP) { 80 | auto members_cpp = 81 | static_cast(ts.second->data); 82 | rmw_iceoryx_cpp::details_cpp::deserializeResponse(serialized_msg, members_cpp, ros_message); 83 | } else if (ts.first == TypeSupportLanguage::C) { 84 | auto members_c = 85 | static_cast(ts.second->data); 86 | rmw_iceoryx_cpp::details_c::deserializeResponse(serialized_msg, members_c, ros_message); 87 | } 88 | } 89 | 90 | } // namespace rmw_iceoryx_cpp 91 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/internal/iceoryx_generate_gid.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2022 - 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "rmw/rmw.h" 17 | #include "rmw/types.h" 18 | #include "rmw/impl/cpp/macros.hpp" 19 | 20 | #include "iceoryx_posh/popo/untyped_publisher.hpp" 21 | #include "iceoryx_posh/popo/untyped_client.hpp" 22 | 23 | rmw_gid_t generate_publisher_gid(iox::popo::UntypedPublisher * const publisher) 24 | { 25 | rmw_gid_t gid; 26 | gid.implementation_identifier = rmw_get_implementation_identifier(); 27 | memset(gid.data, 0, RMW_GID_STORAGE_SIZE); 28 | 29 | iox::popo::UniquePortId typed_uid = publisher->getUid(); 30 | iox::popo::UniquePortId::value_type uid = 31 | static_cast(typed_uid); 32 | size_t size = sizeof(uid); 33 | 34 | if (!typed_uid.isValid() || size > RMW_GID_STORAGE_SIZE) { 35 | RMW_SET_ERROR_MSG("Could not generated client gid"); 36 | return gid; 37 | } 38 | memcpy(gid.data, &uid, size); 39 | 40 | return gid; 41 | } 42 | 43 | rmw_gid_t generate_client_gid(iox::popo::UntypedClient * const client) 44 | { 45 | rmw_gid_t gid; 46 | gid.implementation_identifier = rmw_get_implementation_identifier(); 47 | memset(gid.data, 0, RMW_GID_STORAGE_SIZE); 48 | 49 | iox::popo::UniquePortId typed_uid = client->getUid(); 50 | iox::popo::UniquePortId::value_type uid = 51 | static_cast(typed_uid); 52 | size_t size = sizeof(uid); 53 | 54 | if (!typed_uid.isValid() || size > RMW_GID_STORAGE_SIZE) { 55 | RMW_SET_ERROR_MSG("Could not generated publisher gid"); 56 | return gid; 57 | } 58 | memcpy(gid.data, &uid, size); 59 | 60 | return gid; 61 | } 62 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 by ZhenshengLee. All rights reserved. 3 | // Copyright (c) 2023 by Apex.AI Inc. All rights reserved. 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "rcpputils/split.hpp" 23 | 24 | #include "rosidl_typesupport_cpp/message_type_support.hpp" 25 | #include "rosidl_typesupport_cpp/service_type_support.hpp" 26 | 27 | #include "rosidl_typesupport_introspection_c/field_types.h" 28 | #include "rosidl_typesupport_introspection_c/identifier.h" 29 | #include "rosidl_typesupport_introspection_c/message_introspection.h" 30 | 31 | #include "rosidl_typesupport_introspection_cpp/field_types.hpp" 32 | #include "rosidl_typesupport_introspection_cpp/identifier.hpp" 33 | #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" 34 | 35 | #include "rmw/error_handling.h" 36 | 37 | #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" 38 | #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" 39 | 40 | static constexpr char ARA_DELIMITER[] = "_ara_msgs/msg/"; 41 | static constexpr char ROS2_EVENT_NAME[] = "data"; 42 | static constexpr char ICEORYX_INTROSPECTION_SERVICE[] = "Introspection"; 43 | static constexpr char ICEORYX_INTROSPECTION_MSG_PACKAGE[] = "iceoryx_introspection_msgs"; 44 | 45 | inline std::string to_message_type(const std::string & in) 46 | { 47 | std::string return_value(in); 48 | 49 | auto pos = return_value.find("__"); 50 | if (pos == std::string::npos) { 51 | pos = return_value.find("::"); 52 | } 53 | 54 | return_value.replace(pos, 2, "/"); 55 | return return_value; 56 | } 57 | 58 | inline void extract_type( 59 | const rosidl_message_type_support_t * type_support, 60 | std::string & package_name, 61 | std::string & type_name) 62 | { 63 | package_name = to_message_type(rmw_iceoryx_cpp::iceoryx_get_message_namespace(type_support)); 64 | type_name = rmw_iceoryx_cpp::iceoryx_get_message_name(type_support); 65 | } 66 | 67 | inline void extract_type( 68 | const rosidl_service_type_support_t * type_support, 69 | std::string & package_name, 70 | std::string & type_name) 71 | { 72 | package_name = to_message_type(rmw_iceoryx_cpp::iceoryx_get_service_namespace(type_support)); 73 | type_name = rmw_iceoryx_cpp::iceoryx_get_service_name(type_support); 74 | } 75 | 76 | namespace rmw_iceoryx_cpp 77 | { 78 | 79 | std::tuple 80 | get_name_n_space_from_node_full_name( 81 | const std::string & node_full_name) 82 | { 83 | auto pos = node_full_name.rfind("/"); 84 | 85 | auto node_name = node_full_name.substr(pos + 1, node_full_name.size()); 86 | auto node_namespace = node_full_name.substr(0, pos); 87 | 88 | return std::make_tuple(node_name, node_namespace); 89 | } 90 | 91 | std::tuple 92 | get_name_n_type_from_service_description( 93 | const std::string & service, 94 | const std::string & instance, 95 | const std::string & event) 96 | { 97 | // Filter out inbuilt introspection topics 98 | if (service.find(ICEORYX_INTROSPECTION_SERVICE) != std::string::npos) { 99 | // iceoryx built-in topic handling 100 | // mark as hidden with leading `_` 101 | std::string delimiter_msg = "_iceoryx"; 102 | 103 | return std::make_tuple( 104 | "/" + delimiter_msg + "/" + instance + "/" + service + "/" + event, 105 | std::string(ICEORYX_INTROSPECTION_MSG_PACKAGE) + "/msg/" + event); 106 | } 107 | 108 | // ROS 2.0 Naming 109 | if (event == ROS2_EVENT_NAME) { 110 | // we simply throw away `event` as it was artificially introduced anyway. 111 | return std::make_tuple(instance, service); 112 | } 113 | 114 | // ARA Naming 115 | std::string service_lowercase = service; 116 | std::transform( 117 | service_lowercase.begin(), service_lowercase.end(), 118 | service_lowercase.begin(), ::tolower); 119 | 120 | return std::make_tuple( 121 | "/" + instance + "/" + service + "/" + event, 122 | service_lowercase + ARA_DELIMITER + event); 123 | } 124 | 125 | std::tuple 126 | get_service_description_from_name_n_type( 127 | const std::string & topic_name, 128 | const std::string & type_name) 129 | { 130 | // Filter out inbuilt introspection topics 131 | auto position_introspection_msgs = type_name.find(ICEORYX_INTROSPECTION_MSG_PACKAGE); 132 | if (position_introspection_msgs != std::string::npos) { 133 | auto tokens = rcpputils::split(topic_name, '/', true); // skip_empty for leading `/` 134 | if (tokens.size() != 4) { // ['_iceoryx', , , ] 135 | throw std::runtime_error(std::string("inbuilt topic name is malformed: ") + topic_name); 136 | } 137 | return std::make_tuple(tokens[2], tokens[1], tokens[3]); 138 | } 139 | 140 | auto position_ara_delimiter = type_name.find(ARA_DELIMITER); 141 | 142 | // ROS 2.0 Naming 143 | if (position_ara_delimiter == std::string::npos) { 144 | return std::make_tuple(type_name, topic_name, ROS2_EVENT_NAME); 145 | } 146 | 147 | // ARA Naming 148 | // Due to ros package naming conventions the service name packed into 149 | // the type name had to be lowercase 150 | auto service_lowercase = type_name.substr(0, position_ara_delimiter); 151 | 152 | std::string topic_name_lowercase = topic_name; 153 | std::transform( 154 | topic_name_lowercase.begin(), topic_name_lowercase.end(), 155 | topic_name_lowercase.begin(), ::tolower); 156 | 157 | // Find the lowercase service name in the lowercased topic name 158 | auto position_package_name = topic_name_lowercase.find(service_lowercase); 159 | 160 | if (position_package_name == std::string::npos) { 161 | throw std::runtime_error("message topic and type are inconsistent"); 162 | } 163 | 164 | // Get the mixed uppercase and lowercase service name 165 | // knowing the strings position in the topic name 166 | auto service = topic_name.substr(position_package_name, service_lowercase.length()); 167 | auto instance = topic_name.substr(1, position_package_name - 2); 168 | auto event = type_name.substr(position_ara_delimiter + strlen(ARA_DELIMITER), type_name.size()); 169 | 170 | return std::make_tuple(service, instance, event); 171 | } 172 | 173 | iox::capro::ServiceDescription make_service_description( 174 | const std::string & topic_name, 175 | const std::string & type_name) 176 | { 177 | auto serviceDescriptionTuple = get_service_description_from_name_n_type(topic_name, type_name); 178 | 179 | return iox::capro::ServiceDescription( 180 | iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<0>(serviceDescriptionTuple)), 181 | iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<1>(serviceDescriptionTuple)), 182 | iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<2>(serviceDescriptionTuple))); 183 | } 184 | 185 | iox::capro::ServiceDescription 186 | get_iceoryx_service_description( 187 | const std::string & topic_name, 188 | const rosidl_message_type_support_t * type_supports) 189 | { 190 | std::string package_name; 191 | std::string type_name; 192 | extract_type(type_supports, package_name, type_name); 193 | type_name = package_name + "/" + type_name; 194 | 195 | return make_service_description(topic_name, type_name); 196 | } 197 | 198 | iox::capro::ServiceDescription 199 | get_iceoryx_service_description( 200 | const std::string & topic_name, 201 | const rosidl_service_type_support_t * type_supports) 202 | { 203 | std::string package_name; 204 | std::string type_name; 205 | extract_type(type_supports, package_name, type_name); 206 | type_name = package_name + "/" + type_name; 207 | 208 | return make_service_description(topic_name, type_name); 209 | } 210 | 211 | } // namespace rmw_iceoryx_cpp 212 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/internal/iceoryx_serialization_common.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef INTERNAL__ICEORYX_SERIALIZATION_COMMON_HPP_ 16 | #define INTERNAL__ICEORYX_SERIALIZATION_COMMON_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace rmw_iceoryx_cpp 25 | { 26 | 27 | #define DEBUG_LOG 0 28 | 29 | static inline void debug_log(const char * format, ...) 30 | { 31 | #if DEBUG_LOG 32 | va_list args; 33 | va_start(args, format); 34 | fprintf(stderr, "[READING] "); 35 | vfprintf(stderr, format, args); 36 | va_end(args); 37 | #else 38 | (void) format; 39 | #endif 40 | } 41 | 42 | inline void push_sequence_size(std::vector & payloadVector, uint32_t array_size) 43 | { 44 | const uint32_t check = 101; 45 | const char * sizePtr = reinterpret_cast(&array_size); 46 | const char * checkPtr = reinterpret_cast(&check); 47 | payloadVector.insert(payloadVector.end(), checkPtr, checkPtr + sizeof(check)); 48 | payloadVector.insert(payloadVector.end(), sizePtr, sizePtr + sizeof(array_size)); 49 | } 50 | 51 | inline std::pair pop_sequence_size(const char * serialized_msg) 52 | { 53 | // This is 64 bit aligned 54 | // REVIEW: Please discuss 55 | const uint32_t array_check = *reinterpret_cast(serialized_msg); 56 | serialized_msg += sizeof(array_check); 57 | const uint32_t array_size = *reinterpret_cast(serialized_msg); 58 | serialized_msg += sizeof(array_size); 59 | 60 | if (array_check != 101) { 61 | throw std::runtime_error("can't load array size: check failed"); 62 | } 63 | return std::make_pair(serialized_msg, array_size); 64 | } 65 | 66 | namespace details_c 67 | { 68 | namespace traits 69 | { 70 | 71 | template 72 | struct sequence_type; 73 | 74 | template<> 75 | struct sequence_type 76 | { 77 | using type = rosidl_runtime_c__boolean__Sequence; 78 | }; 79 | 80 | template<> 81 | struct sequence_type 82 | { 83 | using type = rosidl_runtime_c__char__Sequence; 84 | }; 85 | 86 | template<> 87 | struct sequence_type 88 | { 89 | using type = rosidl_runtime_c__float__Sequence; 90 | }; 91 | 92 | template<> 93 | struct sequence_type 94 | { 95 | using type = rosidl_runtime_c__double__Sequence; 96 | }; 97 | 98 | template<> 99 | struct sequence_type 100 | { 101 | using type = rosidl_runtime_c__int8__Sequence; 102 | }; 103 | 104 | template<> 105 | struct sequence_type 106 | { 107 | using type = rosidl_runtime_c__uint8__Sequence; 108 | }; 109 | 110 | template<> 111 | struct sequence_type 112 | { 113 | using type = rosidl_runtime_c__int16__Sequence; 114 | }; 115 | 116 | template<> 117 | struct sequence_type 118 | { 119 | using type = rosidl_runtime_c__uint16__Sequence; 120 | }; 121 | 122 | template<> 123 | struct sequence_type 124 | { 125 | using type = rosidl_runtime_c__int32__Sequence; 126 | }; 127 | 128 | template<> 129 | struct sequence_type 130 | { 131 | using type = rosidl_runtime_c__uint32__Sequence; 132 | }; 133 | 134 | template<> 135 | struct sequence_type 136 | { 137 | using type = rosidl_runtime_c__int64__Sequence; 138 | }; 139 | 140 | template<> 141 | struct sequence_type 142 | { 143 | using type = rosidl_runtime_c__uint64__Sequence; 144 | }; 145 | 146 | template<> 147 | struct sequence_type 148 | { 149 | using type = rosidl_runtime_c__String__Sequence; 150 | }; 151 | 152 | } // namespace traits 153 | } // namespace details_c 154 | } // namespace rmw_iceoryx_cpp 155 | #endif // INTERNAL__ICEORYX_SERIALIZATION_COMMON_HPP_ 156 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 - 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include "rosidl_typesupport_cpp/message_type_support.hpp" 19 | 20 | #include "rosidl_typesupport_introspection_c/identifier.h" 21 | #include "rosidl_typesupport_introspection_c/message_introspection.h" 22 | #include "rosidl_typesupport_introspection_c/service_introspection.h" 23 | 24 | #include "rosidl_typesupport_introspection_cpp/identifier.hpp" 25 | #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" 26 | #include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" 27 | 28 | #include "rcutils/error_handling.h" 29 | 30 | #include "rmw_iceoryx_cpp/iceoryx_serialize.hpp" 31 | #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" 32 | 33 | #include "./iceoryx_serialize_typesupport_c.hpp" 34 | #include "./iceoryx_serialize_typesupport_cpp.hpp" 35 | 36 | namespace rmw_iceoryx_cpp 37 | { 38 | 39 | void serialize( 40 | const void * ros_message, 41 | const rosidl_message_type_support_t * type_supports, 42 | std::vector & payload_vector) 43 | { 44 | auto ts = get_type_support(type_supports); 45 | 46 | if (ts.first == TypeSupportLanguage::CPP) { 47 | auto members = 48 | static_cast(ts.second->data); 49 | rmw_iceoryx_cpp::details_cpp::serialize(ros_message, members, payload_vector); 50 | } else if (ts.first == TypeSupportLanguage::C) { 51 | auto members = 52 | static_cast(ts.second->data); 53 | rmw_iceoryx_cpp::details_c::serialize(ros_message, members, payload_vector); 54 | } 55 | } 56 | 57 | void serializeRequest( 58 | const void * ros_message, 59 | const rosidl_service_type_support_t * type_supports, 60 | std::vector & payload_vector) 61 | { 62 | auto ts = get_type_support(type_supports); 63 | 64 | if (ts.first == TypeSupportLanguage::CPP) { 65 | auto members = 66 | static_cast(ts.second->data); 67 | rmw_iceoryx_cpp::details_cpp::serialize(ros_message, members->request_members_, payload_vector); 68 | } else if (ts.first == TypeSupportLanguage::C) { 69 | auto members = 70 | static_cast(ts.second->data); 71 | rmw_iceoryx_cpp::details_c::serialize(ros_message, members->request_members_, payload_vector); 72 | } 73 | } 74 | 75 | void serializeResponse( 76 | const void * ros_message, 77 | const rosidl_service_type_support_t * type_supports, 78 | std::vector & payload_vector) 79 | { 80 | auto ts = get_type_support(type_supports); 81 | 82 | if (ts.first == TypeSupportLanguage::CPP) { 83 | auto members = 84 | static_cast(ts.second->data); 85 | rmw_iceoryx_cpp::details_cpp::serialize( 86 | ros_message, members->response_members_, 87 | payload_vector); 88 | } else if (ts.first == TypeSupportLanguage::C) { 89 | auto members = 90 | static_cast(ts.second->data); 91 | rmw_iceoryx_cpp::details_c::serialize(ros_message, members->response_members_, payload_vector); 92 | } 93 | } 94 | 95 | } // namespace rmw_iceoryx_cpp 96 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/internal/iceoryx_serialize_typesupport_c.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef INTERNAL__ICEORYX_SERIALIZE_TYPESUPPORT_C_HPP_ 16 | #define INTERNAL__ICEORYX_SERIALIZE_TYPESUPPORT_C_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "rosidl_runtime_c/string_functions.h" 24 | 25 | #include "rosidl_typesupport_introspection_c/field_types.h" 26 | #include "rosidl_typesupport_introspection_c/message_introspection.h" 27 | 28 | #include "./iceoryx_serialization_common.hpp" 29 | 30 | namespace rmw_iceoryx_cpp 31 | { 32 | namespace details_c 33 | { 34 | 35 | template< 36 | class T, 37 | size_t SizeT = sizeof(T) 38 | > 39 | void serialize_sequence(std::vector & serialized_msg, const char * ros_message_field); 40 | 41 | template< 42 | class T, 43 | size_t SizeT = sizeof(T) 44 | > 45 | void serialize_element( 46 | std::vector & serialized_msg, 47 | const char * ros_message_field) 48 | { 49 | debug_log("serializing data element of %u bytes\n", SizeT); 50 | serialized_msg.insert(serialized_msg.end(), ros_message_field, ros_message_field + SizeT); 51 | } 52 | 53 | template<> 54 | void serialize_element( 55 | std::vector & serialized_msg, 56 | const char * ros_message_field) 57 | { 58 | auto string = reinterpret_cast(ros_message_field); 59 | push_sequence_size(serialized_msg, string->size); 60 | serialized_msg.insert(serialized_msg.end(), string->data, string->data + string->size); 61 | } 62 | 63 | template< 64 | class T, 65 | size_t SizeT = sizeof(T) 66 | > 67 | void serialize_array( 68 | std::vector & serialized_msg, 69 | const char * ros_message_field, 70 | uint32_t size) 71 | { 72 | auto array = reinterpret_cast(ros_message_field); 73 | for (size_t i = 0; i < size; ++i) { 74 | auto data = reinterpret_cast(&array[i]); 75 | serialize_element(serialized_msg, data); 76 | } 77 | } 78 | 79 | template< 80 | class T, 81 | size_t SizeT 82 | > 83 | void serialize_sequence(std::vector & serialized_msg, const char * ros_message_field) 84 | { 85 | auto sequence = 86 | reinterpret_cast::type *>(ros_message_field); 87 | uint32_t sequence_size = sequence->size; 88 | 89 | push_sequence_size(serialized_msg, sequence_size); 90 | 91 | serialize_array(serialized_msg, reinterpret_cast(sequence->data), sequence_size); 92 | } 93 | 94 | template 95 | void serialize_message_field( 96 | const rosidl_typesupport_introspection_c__MessageMember * member, 97 | std::vector & serialized_msg, 98 | const char * ros_message_field) 99 | { 100 | debug_log("serializing message field %s\n", member->name_); 101 | if (!member->is_array_) { 102 | serialize_element(serialized_msg, ros_message_field); 103 | } else if (member->array_size_ > 0 && !member->is_upper_bound_) { 104 | serialize_array(serialized_msg, ros_message_field, member->array_size_); 105 | } else { 106 | serialize_sequence(serialized_msg, ros_message_field); 107 | } 108 | } 109 | 110 | void serialize( 111 | const void * ros_message, 112 | const rosidl_typesupport_introspection_c__MessageMembers * members, 113 | std::vector & serialized_msg) 114 | { 115 | assert(members); 116 | assert(ros_message); 117 | 118 | for (uint32_t i = 0; i < members->member_count_; ++i) { 119 | const auto member = members->members_ + i; 120 | const char * ros_message_field = static_cast(ros_message) + member->offset_; 121 | switch (member->type_id_) { 122 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_BOOL: 123 | serialize_message_field(member, serialized_msg, ros_message_field); 124 | break; 125 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_BYTE: 126 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT8: 127 | serialize_message_field(member, serialized_msg, ros_message_field); 128 | break; 129 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_CHAR: 130 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT8: 131 | serialize_message_field(member, serialized_msg, ros_message_field); 132 | break; 133 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT32: 134 | serialize_message_field(member, serialized_msg, ros_message_field); 135 | break; 136 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT64: 137 | serialize_message_field(member, serialized_msg, ros_message_field); 138 | break; 139 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT16: 140 | serialize_message_field(member, serialized_msg, ros_message_field); 141 | break; 142 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT16: 143 | serialize_message_field(member, serialized_msg, ros_message_field); 144 | break; 145 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT32: 146 | serialize_message_field(member, serialized_msg, ros_message_field); 147 | break; 148 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT32: 149 | serialize_message_field(member, serialized_msg, ros_message_field); 150 | break; 151 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_INT64: 152 | serialize_message_field(member, serialized_msg, ros_message_field); 153 | break; 154 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_UINT64: 155 | serialize_message_field(member, serialized_msg, ros_message_field); 156 | break; 157 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_STRING: 158 | serialize_message_field( 159 | member, serialized_msg, 160 | ros_message_field); 161 | break; 162 | case ::rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE: 163 | { 164 | // Iterate recursively over the complex ROS messages 165 | auto sub_members = 166 | static_cast(member->members_ 167 | ->data); 168 | 169 | const void * subros_message = nullptr; 170 | size_t sequence_size = 0; 171 | size_t sub_members_size = sub_members->size_of_; 172 | // It's a single message 173 | if (!member->is_array_) { 174 | subros_message = ros_message_field; 175 | sequence_size = 1; 176 | // It's a fixed size array of messages 177 | } else if (member->array_size_ > 0 && !member->is_upper_bound_) { 178 | subros_message = ros_message_field; 179 | sequence_size = member->array_size_; 180 | // It's a dynamic sequence of messages 181 | } else { 182 | // Cast current ros_message_field ptr as vector "definition" 183 | auto vector = 184 | reinterpret_cast(ros_message_field); 185 | // Vector size points to content of vector and returns number of bytes 186 | // submembersize is the size of one element in the vector 187 | // (it is provided by type support) 188 | sequence_size = vector->size / sub_members_size; 189 | if (member->is_upper_bound_ && sequence_size > member->array_size_) { 190 | throw std::runtime_error("vector overcomes the maximum length"); 191 | } 192 | // create ptr to content of vector to enable recursion 193 | subros_message = reinterpret_cast(vector->data); 194 | // store the number of elements 195 | push_sequence_size(serialized_msg, sequence_size); 196 | } 197 | 198 | debug_log("serializing message field %s\n", member->name_); 199 | for (auto index = 0u; index < sequence_size; ++index) { 200 | serialize(subros_message, sub_members, serialized_msg); 201 | subros_message = static_cast(subros_message) + sub_members_size; 202 | } 203 | } 204 | break; 205 | default: 206 | throw std::runtime_error("unknown type"); 207 | } 208 | } 209 | } 210 | 211 | } // namespace details_c 212 | } // namespace rmw_iceoryx_cpp 213 | #endif // INTERNAL__ICEORYX_SERIALIZE_TYPESUPPORT_C_HPP_ 214 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_client.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2022 - 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include "rcutils/error_handling.h" 19 | 20 | #include "rmw/impl/cpp/macros.hpp" 21 | #include "rmw/rmw.h" 22 | 23 | #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" 24 | 25 | #include "types/iceoryx_client.hpp" 26 | 27 | extern "C" 28 | { 29 | rmw_client_t * 30 | rmw_create_client( 31 | const rmw_node_t * node, 32 | const rosidl_service_type_support_t * type_supports, 33 | const char * service_name, 34 | const rmw_qos_profile_t * qos_policies) 35 | { 36 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, NULL); 37 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_supports, NULL); 38 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_name, NULL); 39 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, NULL); 40 | 41 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 42 | rmw_create_client 43 | : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); 44 | 45 | // create the iceoryx service description for a sender 46 | auto service_description = 47 | rmw_iceoryx_cpp::get_iceoryx_service_description(service_name, type_supports); 48 | 49 | std::string node_full_name = std::string(node->namespace_) + std::string(node->name); 50 | rmw_client_t * rmw_client = nullptr; 51 | iox::popo::UntypedClient * iceoryx_client = nullptr; 52 | IceoryxClient * iceoryx_client_abstraction = nullptr; 53 | 54 | bool returnOnError = false; 55 | 56 | auto cleanupAfterError = [&]() { 57 | if (rmw_client) { 58 | if (iceoryx_client) { 59 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 60 | iceoryx_client->~UntypedClient(), iox::popo::UntypedClient) 61 | rmw_free(iceoryx_client); 62 | } 63 | if (iceoryx_client_abstraction) { 64 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 65 | iceoryx_client_abstraction->~IceoryxClient(), IceoryxClient) 66 | rmw_free(iceoryx_client_abstraction); 67 | } 68 | if (rmw_client->service_name) { 69 | rmw_free(const_cast(rmw_client->service_name)); 70 | } 71 | rmw_client_free(rmw_client); 72 | returnOnError = true; 73 | } 74 | }; 75 | 76 | rmw_client = rmw_client_allocate(); 77 | if (!rmw_client) { 78 | RMW_SET_ERROR_MSG("failed to allocate memory for client"); 79 | cleanupAfterError(); 80 | return nullptr; 81 | } 82 | 83 | iceoryx_client = 84 | static_cast(rmw_allocate( 85 | sizeof(iox::popo::UntypedClient))); 86 | if (!iceoryx_client) { 87 | RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx client"); 88 | cleanupAfterError(); 89 | return nullptr; 90 | } 91 | 92 | RMW_TRY_PLACEMENT_NEW( 93 | iceoryx_client, iceoryx_client, 94 | cleanupAfterError(), iox::popo::UntypedClient, service_description, 95 | iox::popo::ClientOptions{ 96 | qos_policies->depth, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); 97 | if (returnOnError) { 98 | return nullptr; 99 | } 100 | 101 | iceoryx_client->connect(); 102 | 103 | iceoryx_client_abstraction = 104 | static_cast(rmw_allocate(sizeof(IceoryxClient))); 105 | if (!iceoryx_client_abstraction) { 106 | RMW_SET_ERROR_MSG("failed to allocate memory for rmw iceoryx publisher"); 107 | cleanupAfterError(); 108 | return nullptr; 109 | } 110 | RMW_TRY_PLACEMENT_NEW( 111 | iceoryx_client_abstraction, iceoryx_client_abstraction, 112 | cleanupAfterError(), IceoryxClient, type_supports, iceoryx_client); 113 | if (returnOnError) { 114 | return nullptr; 115 | } 116 | 117 | rmw_client->implementation_identifier = rmw_get_implementation_identifier(); 118 | rmw_client->data = iceoryx_client_abstraction; 119 | 120 | rmw_client->service_name = 121 | static_cast(rmw_allocate(sizeof(char) * strlen(service_name) + 1)); 122 | if (!rmw_client->service_name) { 123 | RMW_SET_ERROR_MSG("failed to allocate memory for service name"); 124 | cleanupAfterError(); 125 | return nullptr; 126 | } 127 | memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); 128 | return rmw_client; 129 | } 130 | 131 | rmw_ret_t 132 | rmw_destroy_client( 133 | rmw_node_t * node, 134 | rmw_client_t * client) 135 | { 136 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 137 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR); 138 | 139 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 140 | rmw_destroy_client 141 | : client, client->implementation_identifier, 142 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 143 | 144 | rmw_ret_t result = RMW_RET_OK; 145 | 146 | IceoryxClient * iceoryx_client_abstraction = static_cast(client->data); 147 | if (iceoryx_client_abstraction) { 148 | if (iceoryx_client_abstraction->iceoryx_client_) { 149 | RMW_TRY_DESTRUCTOR( 150 | iceoryx_client_abstraction->iceoryx_client_->~UntypedClient(), 151 | iceoryx_client_abstraction->iceoryx_client_, 152 | result = RMW_RET_ERROR) 153 | rmw_free(iceoryx_client_abstraction->iceoryx_client_); 154 | } 155 | RMW_TRY_DESTRUCTOR( 156 | iceoryx_client_abstraction->~IceoryxClient(), 157 | iceoryx_client_abstraction, 158 | result = RMW_RET_ERROR) 159 | rmw_free(iceoryx_client_abstraction); 160 | } 161 | 162 | client->data = nullptr; 163 | 164 | rmw_free(const_cast(client->service_name)); 165 | client->service_name = nullptr; 166 | 167 | rmw_client_free(client); 168 | return result; 169 | } 170 | 171 | rmw_ret_t rmw_client_request_publisher_get_actual_qos( 172 | const rmw_client_t * client, 173 | rmw_qos_profile_t * qos) 174 | { 175 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); 176 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); 177 | 178 | *qos = rmw_qos_profile_default; 179 | 180 | return RMW_RET_OK; 181 | } 182 | 183 | rmw_ret_t rmw_client_response_subscription_get_actual_qos( 184 | const rmw_client_t * client, 185 | rmw_qos_profile_t * qos) 186 | { 187 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); 188 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); 189 | 190 | *qos = rmw_qos_profile_default; 191 | 192 | return RMW_RET_OK; 193 | } 194 | 195 | rmw_ret_t rmw_client_set_on_new_response_callback( 196 | rmw_client_t * client, 197 | rmw_event_callback_t callback, 198 | const void * user_data) 199 | { 200 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); 201 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(callback, RMW_RET_INVALID_ARGUMENT); 202 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(user_data, RMW_RET_INVALID_ARGUMENT); 203 | 204 | return RMW_RET_UNSUPPORTED; 205 | } 206 | 207 | rmw_ret_t 208 | rmw_get_gid_for_client(const rmw_client_t * client, rmw_gid_t * gid) 209 | { 210 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); 211 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT); 212 | 213 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 214 | rmw_get_gid_for_client 215 | : client, client->implementation_identifier, 216 | rmw_get_implementation_identifier(), 217 | return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 218 | 219 | IceoryxClient * iceoryx_client_abstraction = static_cast(client->data); 220 | 221 | if (!iceoryx_client_abstraction) { 222 | RMW_SET_ERROR_MSG("client info handle is null"); 223 | return RMW_RET_INVALID_ARGUMENT; 224 | } 225 | *gid = iceoryx_client_abstraction->gid_; 226 | return RMW_RET_OK; 227 | } 228 | } // extern "C" 229 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_compare_guids_equal.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "rcutils/error_handling.h" 18 | #include "rmw/impl/cpp/macros.hpp" 19 | #include "rmw/rmw.h" 20 | 21 | 22 | extern "C" 23 | { 24 | rmw_ret_t 25 | rmw_compare_gids_equal( 26 | const rmw_gid_t * gid1, 27 | const rmw_gid_t * gid2, 28 | bool * result) 29 | { 30 | *result = false; 31 | 32 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(gid1, RMW_RET_INVALID_ARGUMENT); 33 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 34 | gid1, 35 | gid1->implementation_identifier, 36 | rmw_get_implementation_identifier(), 37 | return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 38 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(gid2, RMW_RET_INVALID_ARGUMENT); 39 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 40 | gid2, 41 | gid2->implementation_identifier, 42 | rmw_get_implementation_identifier(), 43 | return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 44 | 45 | if (std::memcmp(gid1->data, gid2->data, RMW_GID_STORAGE_SIZE) == 0) { 46 | *result = true; 47 | } 48 | return RMW_RET_OK; 49 | } 50 | } // extern "C" 51 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_count.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rcutils/error_handling.h" 16 | #include "rmw/impl/cpp/macros.hpp" 17 | #include "rmw/rmw.h" 18 | 19 | #include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp" 20 | 21 | extern "C" 22 | { 23 | rmw_ret_t 24 | rmw_count_publishers( 25 | const rmw_node_t * node, 26 | const char * topic_name, 27 | size_t * count) 28 | { 29 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 30 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_ERROR); 31 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_ERROR); 32 | 33 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 34 | rmw_count_publishers 35 | : node, node->implementation_identifier, 36 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 37 | 38 | auto nodes_and_publishers = rmw_iceoryx_cpp::get_nodes_and_publishers(); 39 | 40 | size_t counter = 0; 41 | 42 | for (auto node : nodes_and_publishers) { 43 | for (auto topic : node.second) { 44 | if (topic == topic_name) { 45 | ++counter; 46 | } 47 | } 48 | } 49 | 50 | *count = counter; 51 | 52 | return RMW_RET_OK; 53 | } 54 | 55 | rmw_ret_t 56 | rmw_count_subscribers( 57 | const rmw_node_t * node, 58 | const char * topic_name, 59 | size_t * count) 60 | { 61 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 62 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_ERROR); 63 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_ERROR); 64 | 65 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 66 | rmw_count_subscribers 67 | : node, node->implementation_identifier, 68 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 69 | 70 | auto nodes_and_subscribers = rmw_iceoryx_cpp::get_nodes_and_subscribers(); 71 | 72 | size_t counter = 0; 73 | 74 | for (auto node : nodes_and_subscribers) { 75 | for (auto topic : node.second) { 76 | if (topic == topic_name) { 77 | ++counter; 78 | } 79 | } 80 | } 81 | 82 | *count = counter; 83 | 84 | return RMW_RET_OK; 85 | } 86 | 87 | rmw_ret_t 88 | rmw_subscription_count_matched_publishers( 89 | const rmw_subscription_t * subscription, 90 | size_t * publisher_count) 91 | { 92 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); 93 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher_count, RMW_RET_ERROR); 94 | 95 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 96 | rmw_subscription_count_matched_publishers 97 | : subscription, subscription->implementation_identifier, 98 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 99 | 100 | auto nodes_and_publishers = rmw_iceoryx_cpp::get_nodes_and_publishers(); 101 | 102 | size_t counter = 0; 103 | 104 | for (auto node : nodes_and_publishers) { 105 | for (auto topic : node.second) { 106 | if (topic == subscription->topic_name) { 107 | ++counter; 108 | } 109 | } 110 | } 111 | 112 | *publisher_count = counter; 113 | 114 | return RMW_RET_OK; 115 | } 116 | 117 | rmw_ret_t 118 | rmw_publisher_count_matched_subscriptions( 119 | const rmw_publisher_t * publisher, 120 | size_t * subscription_count) 121 | { 122 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); 123 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription_count, RMW_RET_ERROR); 124 | 125 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 126 | rmw_publisher_count_matched_subscriptions 127 | : publisher, publisher->implementation_identifier, 128 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 129 | 130 | auto nodes_and_subscribers = rmw_iceoryx_cpp::get_nodes_and_subscribers(); 131 | 132 | size_t counter = 0; 133 | 134 | for (auto node : nodes_and_subscribers) { 135 | for (auto topic : node.second) { 136 | if (topic == publisher->topic_name) { 137 | ++counter; 138 | } 139 | } 140 | } 141 | 142 | *subscription_count = counter; 143 | return RMW_RET_OK; 144 | } 145 | } // extern "C" 146 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_dynamic_type_support.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 by Apex.AI Inc. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw/impl/cpp/macros.hpp" 16 | #include "rmw/rmw.h" 17 | 18 | extern "C" 19 | { 20 | rmw_ret_t 21 | rmw_take_dynamic_message( 22 | const rmw_subscription_t * subscription, 23 | rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, 24 | bool * taken, 25 | rmw_subscription_allocation_t * allocation) 26 | { 27 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); 28 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(dynamic_message, RMW_RET_ERROR); 29 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_ERROR); 30 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocation, RMW_RET_ERROR); 31 | 32 | RMW_SET_ERROR_MSG("rmw_take_dynamic_message is not supported in iceoryx"); 33 | return RMW_RET_UNSUPPORTED; 34 | } 35 | 36 | rmw_ret_t 37 | rmw_take_dynamic_message_with_info( 38 | const rmw_subscription_t * subscription, 39 | rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, 40 | bool * taken, 41 | rmw_message_info_t * message_info, 42 | rmw_subscription_allocation_t * allocation) 43 | { 44 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); 45 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(dynamic_message, RMW_RET_ERROR); 46 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_ERROR); 47 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_ERROR); 48 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocation, RMW_RET_ERROR); 49 | 50 | RMW_SET_ERROR_MSG("rmw_take_dynamic_message_with_info is not supported in iceoryx"); 51 | return RMW_RET_UNSUPPORTED; 52 | } 53 | 54 | rmw_ret_t 55 | rmw_serialization_support_init( 56 | const char * serialization_lib_name, 57 | rcutils_allocator_t * allocator, 58 | rosidl_dynamic_typesupport_serialization_support_t * serialization_support) 59 | { 60 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(serialization_lib_name, RMW_RET_ERROR); 61 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 62 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(serialization_support, RMW_RET_ERROR); 63 | 64 | RMW_SET_ERROR_MSG("rmw_serialization_support_init is not supported in iceoryx"); 65 | return RMW_RET_UNSUPPORTED; 66 | } 67 | } // extern "C" 68 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_event.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2022 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "rcutils/error_handling.h" 17 | #include "rmw/event.h" 18 | #include "rmw/impl/cpp/macros.hpp" 19 | #include "rmw/rmw.h" 20 | 21 | extern "C" 22 | { 23 | rmw_ret_t 24 | rmw_publisher_event_init( 25 | rmw_event_t * rmw_event, 26 | const rmw_publisher_t * publisher, 27 | rmw_event_type_t event_type) 28 | { 29 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_ERROR); 30 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); 31 | (void) event_type; 32 | 33 | /// @todo add publisher events support 34 | return RMW_RET_UNSUPPORTED; 35 | } 36 | 37 | rmw_ret_t 38 | rmw_subscription_event_init( 39 | rmw_event_t * rmw_event, 40 | const rmw_subscription_t * subscription, 41 | rmw_event_type_t event_type) 42 | { 43 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_ERROR); 44 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); 45 | (void) event_type; 46 | 47 | /// @todo add subscription events support 48 | return RMW_RET_UNSUPPORTED; 49 | } 50 | 51 | rmw_ret_t rmw_event_set_callback( 52 | rmw_event_t * event, 53 | rmw_event_callback_t callback, 54 | const void * user_data) 55 | { 56 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(event, RMW_RET_INVALID_ARGUMENT); 57 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(callback, RMW_RET_INVALID_ARGUMENT); 58 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(user_data, RMW_RET_INVALID_ARGUMENT); 59 | 60 | return RMW_RET_UNSUPPORTED; 61 | } 62 | } // extern "C" 63 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_feature.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022 by Apex.AI Inc. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw/features.h" 16 | 17 | bool rmw_feature_supported(rmw_feature_t feature) 18 | { 19 | static_cast(feature); 20 | return false; 21 | } 22 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_get_gid_for_publisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rcutils/error_handling.h" 16 | 17 | #include "rmw/impl/cpp/macros.hpp" 18 | 19 | #include "rmw/rmw.h" 20 | 21 | #include "./types/iceoryx_publisher.hpp" 22 | 23 | extern "C" 24 | { 25 | rmw_ret_t 26 | rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) 27 | { 28 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); 29 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT); 30 | 31 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 32 | rmw_get_gid_for_publisher 33 | : publisher, publisher->implementation_identifier, 34 | rmw_get_implementation_identifier(), 35 | return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 36 | 37 | IceoryxPublisher * iceoryx_publisher = static_cast(publisher->data); 38 | if (!iceoryx_publisher) { 39 | RMW_SET_ERROR_MSG("publisher info handle is null"); 40 | return RMW_RET_INVALID_ARGUMENT; 41 | } 42 | *gid = iceoryx_publisher->gid_; 43 | return RMW_RET_OK; 44 | } 45 | } // extern "C" 46 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_get_implementation_identifier.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw/rmw.h" 16 | 17 | #include "./iceoryx_identifier.hpp" 18 | 19 | const char * const rmw_iceoryx_cpp_identifier = "rmw_iceoryx_cpp"; 20 | 21 | extern "C" 22 | { 23 | const char * 24 | rmw_get_implementation_identifier() 25 | { 26 | return rmw_iceoryx_cpp_identifier; 27 | } 28 | } // extern "C" 29 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_get_serialization_format.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw/rmw.h" 16 | 17 | const char * const iceoryx_serialization_format = "shared_memory"; 18 | 19 | extern "C" 20 | { 21 | const char * 22 | rmw_get_serialization_format() 23 | { 24 | return iceoryx_serialization_format; 25 | } 26 | } // extern "C" 27 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_get_topic_endpoint_info.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rcutils/error_handling.h" 16 | #include "rmw/get_topic_endpoint_info.h" 17 | #include "rmw/impl/cpp/macros.hpp" 18 | #include "rmw/rmw.h" 19 | #include "rmw/topic_endpoint_info_array.h" 20 | 21 | #include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp" 22 | #include "rmw_iceoryx_cpp/iceoryx_get_topic_endpoint_info.hpp" 23 | 24 | extern "C" { 25 | rmw_ret_t rmw_get_publishers_info_by_topic( 26 | const rmw_node_t * node, rcutils_allocator_t * allocator, 27 | const char * topic_name, bool no_mangle, 28 | rmw_topic_endpoint_info_array_t * publishers_info) 29 | { 30 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 31 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 32 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_ERROR); 33 | (void)no_mangle; 34 | 35 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 36 | rmw_get_topic_names_and_types 37 | : node, node->implementation_identifier, rmw_get_implementation_identifier(), 38 | return RMW_RET_ERROR); 39 | 40 | rmw_ret_t rmw_ret = rmw_topic_endpoint_info_array_check_zero(publishers_info); 41 | if (rmw_ret != RMW_RET_OK) { 42 | return rmw_ret; // error already set 43 | } 44 | 45 | auto iceoryx_publisher_endpoint_info = 46 | rmw_iceoryx_cpp::get_publisher_end_info_of_topic(topic_name); 47 | 48 | return rmw_iceoryx_cpp::fill_rmw_publisher_end_info( 49 | publishers_info, 50 | iceoryx_publisher_endpoint_info, allocator); 51 | } 52 | 53 | rmw_ret_t rmw_get_subscriptions_info_by_topic( 54 | const rmw_node_t * node, rcutils_allocator_t * allocator, 55 | const char * topic_name, bool no_mangle, 56 | rmw_topic_endpoint_info_array_t * subscriptions_info) 57 | { 58 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 59 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 60 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_ERROR); 61 | (void)no_mangle; 62 | 63 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 64 | rmw_get_topic_names_and_types 65 | : node, node->implementation_identifier, rmw_get_implementation_identifier(), 66 | return RMW_RET_ERROR); 67 | 68 | rmw_ret_t rmw_ret = rmw_topic_endpoint_info_array_check_zero(subscriptions_info); 69 | if (rmw_ret != RMW_RET_OK) { 70 | return rmw_ret; // error already set 71 | } 72 | 73 | auto iceoryx_subscriber_endpoint_info = rmw_iceoryx_cpp::get_subscriber_end_info_of_topic( 74 | topic_name); 75 | 76 | return rmw_iceoryx_cpp::fill_rmw_subscriber_end_info( 77 | subscriptions_info, iceoryx_subscriber_endpoint_info, 78 | allocator); 79 | } 80 | } // extern "C" 81 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_guard_condition.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include "rmw/allocators.h" 19 | #include "rmw/error_handling.h" 20 | #include "rmw/impl/cpp/macros.hpp" 21 | #include "rmw/rmw.h" 22 | 23 | #include "./iceoryx_identifier.hpp" 24 | #include "iceoryx_posh/popo/user_trigger.hpp" 25 | 26 | extern "C" 27 | { 28 | rmw_guard_condition_t * 29 | rmw_create_guard_condition(rmw_context_t * context) 30 | { 31 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, nullptr); 32 | 33 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 34 | rmw_create_guard_condition 35 | : context, 36 | context->implementation_identifier, 37 | rmw_get_implementation_identifier(), 38 | /// @todo wwjwood: replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when 39 | /// possible 40 | return nullptr); 41 | 42 | rmw_guard_condition_t * guard_condition = nullptr; 43 | iox::popo::UserTrigger * iceoryx_guard_condition = nullptr; 44 | 45 | guard_condition = rmw_guard_condition_allocate(); 46 | if (!guard_condition) { 47 | RMW_SET_ERROR_MSG("failed to construct guard condition"); 48 | goto fail; 49 | } 50 | guard_condition->implementation_identifier = rmw_get_implementation_identifier(); 51 | 52 | iceoryx_guard_condition = static_cast( 53 | rmw_allocate(sizeof(iox::popo::UserTrigger))); 54 | if (!iceoryx_guard_condition) { 55 | RMW_SET_ERROR_MSG("failed to construct guard condition data"); 56 | goto fail; 57 | } 58 | RMW_TRY_PLACEMENT_NEW( 59 | iceoryx_guard_condition, 60 | iceoryx_guard_condition, 61 | goto fail, 62 | // cppcheck-suppress syntaxError 63 | iox::popo::UserTrigger, ) 64 | guard_condition->data = iceoryx_guard_condition; 65 | 66 | return guard_condition; 67 | 68 | fail: 69 | if (guard_condition) { 70 | if (iceoryx_guard_condition) { 71 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 72 | iceoryx_guard_condition->~UserTrigger(), iceoryx_guard_condition) 73 | rmw_free(iceoryx_guard_condition); 74 | } 75 | rmw_guard_condition_free(guard_condition); 76 | } 77 | 78 | return nullptr; 79 | } 80 | 81 | rmw_ret_t 82 | rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) 83 | { 84 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(guard_condition, RMW_RET_ERROR); 85 | 86 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 87 | rmw_destroy_guard_condition 88 | : guard_condition, 89 | guard_condition->implementation_identifier, 90 | rmw_get_implementation_identifier(), 91 | return RMW_RET_ERROR); 92 | 93 | auto iceoryx_guard_condition = static_cast(guard_condition->data); 94 | 95 | auto result = RMW_RET_OK; 96 | if (iceoryx_guard_condition) { 97 | RMW_TRY_DESTRUCTOR( 98 | iceoryx_guard_condition->~UserTrigger(), 99 | iceoryx_guard_condition, 100 | result = RMW_RET_ERROR) 101 | rmw_free(iceoryx_guard_condition); 102 | } 103 | rmw_guard_condition_free(guard_condition); 104 | 105 | return result; 106 | } 107 | } // extern "C" 108 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_init.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | #include 18 | 19 | #include "iceoryx_posh/runtime/posh_runtime.hpp" 20 | 21 | #include "rcutils/error_handling.h" 22 | #include "rcutils/process.h" 23 | 24 | #include "rmw/error_handling.h" 25 | #include "rmw/impl/cpp/macros.hpp" 26 | #include "rmw/rmw.h" 27 | #include "rmw/types.h" 28 | 29 | extern "C" 30 | { 31 | rmw_ret_t 32 | rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t allocator) 33 | { 34 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); 35 | RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); 36 | if (NULL != init_options->implementation_identifier) { 37 | RMW_SET_ERROR_MSG("expected zero-initialized init_options"); 38 | return RMW_RET_INVALID_ARGUMENT; 39 | } 40 | init_options->instance_id = 0; 41 | init_options->implementation_identifier = rmw_get_implementation_identifier(); 42 | init_options->allocator = allocator; 43 | init_options->impl = nullptr; 44 | 45 | return RMW_RET_OK; 46 | } 47 | 48 | rmw_ret_t 49 | rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) 50 | { 51 | RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); 52 | RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); 53 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 54 | rmw_init_options_copy 55 | : source options, 56 | src->implementation_identifier, 57 | rmw_get_implementation_identifier(), 58 | return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 59 | if (NULL != dst->implementation_identifier) { 60 | RMW_SET_ERROR_MSG("expected zero-initialized dst"); 61 | return RMW_RET_INVALID_ARGUMENT; 62 | } 63 | *dst = *src; 64 | 65 | return RMW_RET_OK; 66 | } 67 | 68 | rmw_ret_t 69 | rmw_init_options_fini(rmw_init_options_t * init_options) 70 | { 71 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); 72 | RCUTILS_CHECK_ALLOCATOR(&(init_options->allocator), return RMW_RET_INVALID_ARGUMENT); 73 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 74 | rmw_init_options_fini 75 | : options, 76 | init_options->implementation_identifier, 77 | rmw_get_implementation_identifier(), 78 | return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 79 | *init_options = rmw_get_zero_initialized_init_options(); 80 | return RMW_RET_OK; 81 | } 82 | 83 | rmw_ret_t 84 | rmw_init(const rmw_init_options_t * options, rmw_context_t * context) 85 | { 86 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); 87 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); 88 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 89 | rmw_init 90 | : options, 91 | options->implementation_identifier, 92 | rmw_get_implementation_identifier(), 93 | return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 94 | context->instance_id = options->instance_id; 95 | context->implementation_identifier = rmw_get_implementation_identifier(); 96 | context->impl = nullptr; 97 | 98 | // create a name for the process to register with the RouDi daemon 99 | rcutils_allocator_t allocator = rcutils_get_default_allocator(); 100 | auto progam_name = rcutils_get_executable_name(allocator); 101 | auto progName = std::string(progam_name); 102 | /// @todo we could check with the introspection topics beforehand if the name is already used 103 | auto name = progName + "_" + std::to_string(getpid()); 104 | 105 | // This call creates a runtime object. 106 | // It regisers with the RouDi daemon and gets the configuration 107 | // for setting up the shared memeory 108 | iox::log::LogManager::GetLogManager().SetDefaultLogLevel(iox::log::LogLevel::kWarn); 109 | iox::runtime::PoshRuntime::initRuntime(iox::RuntimeName_t(iox::cxx::TruncateToCapacity, name)); 110 | 111 | return RMW_RET_OK; 112 | } 113 | 114 | rmw_ret_t 115 | rmw_shutdown(rmw_context_t * context) 116 | { 117 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); 118 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 119 | rmw_shutdown 120 | : context, 121 | context->implementation_identifier, 122 | rmw_get_implementation_identifier(), 123 | return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 124 | 125 | return RMW_RET_OK; 126 | } 127 | 128 | rmw_ret_t 129 | rmw_context_fini(rmw_context_t * context) 130 | { 131 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); 132 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 133 | rmw_context_fini 134 | : context, 135 | context->implementation_identifier, 136 | rmw_get_implementation_identifier(), 137 | return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 138 | // context impl is explicitly supposed to be nullptr for now, see rmw_init() 139 | *context = rmw_get_zero_initialized_context(); 140 | return RMW_RET_OK; 141 | } 142 | } // extern "C" 143 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_logging.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw/impl/cpp/macros.hpp" 16 | #include "rmw/rmw.h" 17 | 18 | extern "C" 19 | { 20 | rmw_ret_t 21 | rmw_set_log_severity(rmw_log_severity_t severity) 22 | { 23 | (void)severity; 24 | 25 | RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support log severities."); 26 | return RMW_RET_UNSUPPORTED; 27 | } 28 | } // extern "C" 29 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_network_flow_endpoint.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw/get_network_flow_endpoints.h" 16 | #include "rcutils/error_handling.h" 17 | #include "rmw/impl/cpp/macros.hpp" 18 | 19 | extern "C" 20 | { 21 | rmw_ret_t 22 | rmw_publisher_get_network_flow_endpoints( 23 | const rmw_publisher_t * publisher, 24 | rcutils_allocator_t * allocator, 25 | rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) 26 | { 27 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); 28 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 29 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(network_flow_endpoint_array, RMW_RET_ERROR); 30 | 31 | RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support get network flow endpoints."); 32 | return RMW_RET_UNSUPPORTED; 33 | } 34 | 35 | rmw_ret_t 36 | rmw_subscription_get_network_flow_endpoints( 37 | const rmw_subscription_t * subscription, 38 | rcutils_allocator_t * allocator, 39 | rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) 40 | { 41 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); 42 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 43 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(network_flow_endpoint_array, RMW_RET_ERROR); 44 | 45 | RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support get network flow endpoints."); 46 | return RMW_RET_UNSUPPORTED; 47 | } 48 | } // extern "C" 49 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include "iceoryx_posh/runtime/node.hpp" 19 | 20 | #include "rmw/allocators.h" 21 | 22 | #include "./types/iceoryx_node.hpp" 23 | 24 | extern "C" 25 | { 26 | rmw_node_t * 27 | rmw_create_node( 28 | rmw_context_t * context, 29 | const char * name, 30 | const char * namespace_) 31 | { 32 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, nullptr); 33 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(name, nullptr); 34 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(namespace_, nullptr); 35 | 36 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 37 | rmw_create_node 38 | : context, context->implementation_identifier, 39 | rmw_get_implementation_identifier(), return nullptr); 40 | 41 | std::string full_name = std::string(namespace_) + std::string(name); 42 | rmw_guard_condition_t * guard_condition = nullptr; 43 | IceoryxGraphChangeNotifier * graph_change_notifier = nullptr; 44 | iox::runtime::Node * iceoryx_runnable = nullptr; 45 | IceoryxNodeInfo * node_info = nullptr; 46 | 47 | rmw_node_t * node_handle = rmw_node_allocate(); 48 | if (!node_handle) { 49 | RMW_SET_ERROR_MSG("failed to allocate memory for node handle"); 50 | goto fail; 51 | } 52 | 53 | node_handle->implementation_identifier = rmw_get_implementation_identifier(); 54 | 55 | guard_condition = rmw_create_guard_condition(context); 56 | if (!guard_condition) { 57 | RMW_SET_ERROR_MSG("failed to create guard condition"); 58 | goto fail; 59 | } 60 | 61 | /// @todo only use one GraphChangeNotifier for all nodes 62 | graph_change_notifier = 63 | static_cast(rmw_allocate(sizeof(IceoryxGraphChangeNotifier))); 64 | if (!graph_change_notifier) { 65 | RMW_SET_ERROR_MSG("failed to allocate memory for graph change notifier"); 66 | goto fail; 67 | } 68 | RMW_TRY_PLACEMENT_NEW( 69 | graph_change_notifier, graph_change_notifier, goto fail, 70 | IceoryxGraphChangeNotifier, guard_condition) 71 | 72 | // allocate iceoryx_runnable 73 | iceoryx_runnable = 74 | static_cast(rmw_allocate( 75 | sizeof(iox::runtime::Node))); 76 | if (!iceoryx_runnable) { 77 | RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx runnable"); 78 | goto fail; 79 | } 80 | RMW_TRY_PLACEMENT_NEW( 81 | iceoryx_runnable, iceoryx_runnable, 82 | goto fail, iox::runtime::Node, 83 | iox::NodeName_t(iox::cxx::TruncateToCapacity, full_name)); 84 | 85 | node_info = static_cast(rmw_allocate(sizeof(IceoryxNodeInfo))); 86 | if (!node_info) { 87 | RMW_SET_ERROR_MSG("failed to allocate memory for node info"); 88 | goto fail; 89 | } 90 | RMW_TRY_PLACEMENT_NEW( 91 | node_info, node_info, goto fail, IceoryxNodeInfo, guard_condition, 92 | graph_change_notifier, iceoryx_runnable) 93 | 94 | node_handle->data = node_info; 95 | 96 | node_handle->name = static_cast(rmw_allocate(sizeof(char) * strlen(name) + 1)); 97 | if (!node_handle->name) { 98 | RMW_SET_ERROR_MSG("failed to allocate memory for node name"); 99 | goto fail; 100 | } 101 | memcpy(const_cast(node_handle->name), name, strlen(name) + 1); 102 | 103 | node_handle->namespace_ = 104 | static_cast(rmw_allocate(sizeof(char) * strlen(namespace_) + 1)); 105 | if (!node_handle->namespace_) { 106 | RMW_SET_ERROR_MSG("failed to allocate memory for node namespace"); 107 | goto fail; 108 | } 109 | memcpy(const_cast(node_handle->namespace_), namespace_, strlen(namespace_) + 1); 110 | 111 | return node_handle; 112 | 113 | fail: 114 | if (node_handle) { 115 | if (graph_change_notifier) { 116 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 117 | graph_change_notifier->~IceoryxGraphChangeNotifier(), IceoryxGraphChangeNotifier) 118 | rmw_free(graph_change_notifier); 119 | } 120 | if (guard_condition) { 121 | if (RMW_RET_OK != rmw_destroy_guard_condition(guard_condition)) { 122 | RMW_SET_ERROR_MSG("failed to delete graph guard condition"); 123 | } 124 | } 125 | if (iceoryx_runnable) { 126 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 127 | iceoryx_runnable->~Node(), iox::runtime::Node) 128 | rmw_free(iceoryx_runnable); 129 | } 130 | if (node_info) { 131 | rmw_free(node_info); 132 | } 133 | if (node_handle->name) { 134 | rmw_free(const_cast(node_handle->name)); 135 | } 136 | if (node_handle->namespace_) { 137 | rmw_free(const_cast(node_handle->namespace_)); 138 | } 139 | rmw_node_free(node_handle); 140 | } 141 | return nullptr; 142 | } 143 | 144 | rmw_ret_t 145 | rmw_destroy_node(rmw_node_t * node) 146 | { 147 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 148 | 149 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 150 | rmw_destroy_node 151 | : node, node->implementation_identifier, 152 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 153 | 154 | rmw_ret_t result = RMW_RET_OK; 155 | 156 | IceoryxNodeInfo * node_info = static_cast(node->data); 157 | if (node_info) { 158 | if (node_info->graph_change_notifier_) { 159 | RMW_TRY_DESTRUCTOR( 160 | node_info->graph_change_notifier_->~IceoryxGraphChangeNotifier(), 161 | node_info->graph_change_notifier_, 162 | result = RMW_RET_ERROR) 163 | rmw_free(node_info->graph_change_notifier_); 164 | } 165 | if (RMW_RET_OK != rmw_destroy_guard_condition(node_info->guard_condition_)) { 166 | RMW_SET_ERROR_MSG("failed to delete graph guard condition"); 167 | result = RMW_RET_ERROR; 168 | } 169 | if (node_info->iceoryx_runnable_) { 170 | RMW_TRY_DESTRUCTOR( 171 | node_info->iceoryx_runnable_->~Node(), 172 | node_info->iceoryx_runnable_, 173 | result = RMW_RET_ERROR) 174 | rmw_free(node_info->iceoryx_runnable_); 175 | } 176 | RMW_TRY_DESTRUCTOR( 177 | node_info->~IceoryxNodeInfo(), 178 | node_info_, 179 | result = RMW_RET_ERROR) 180 | rmw_free(node_info); 181 | } 182 | node->data = nullptr; 183 | 184 | rmw_free(const_cast(node->name)); 185 | node->name = nullptr; 186 | 187 | rmw_free(const_cast(node->namespace_)); 188 | node->namespace_ = nullptr; 189 | 190 | rmw_node_free(node); 191 | 192 | return result; 193 | } 194 | 195 | rmw_ret_t 196 | rmw_node_assert_liveliness(const rmw_node_t * node) 197 | { 198 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 199 | 200 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 201 | rmw_node_assert_liveliness 202 | : node, node->implementation_identifier, 203 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 204 | 205 | /// @todo poehnl: Currently the heartbeat is used that every registered process in iceoryx sends 206 | /// later this should be extended to a user triggered liveliness that can be send for a runnable 207 | 208 | return RMW_RET_OK; 209 | } 210 | 211 | const rmw_guard_condition_t * 212 | rmw_node_get_graph_guard_condition(const rmw_node_t * node) 213 | { 214 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, nullptr); 215 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); 216 | 217 | IceoryxNodeInfo * node_info = static_cast(node->data); 218 | 219 | return node_info->guard_condition_; 220 | } 221 | } // extern "C" 222 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_node_info_and_types.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rcutils/error_handling.h" 16 | #include "rcutils/logging_macros.h" 17 | #include "rcutils/strdup.h" 18 | 19 | #include "rmw/impl/cpp/macros.hpp" 20 | #include "rmw/rmw.h" 21 | #include "rmw/get_node_info_and_types.h" 22 | #include "rmw/names_and_types.h" 23 | 24 | #include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp" 25 | 26 | extern "C" 27 | { 28 | rmw_ret_t 29 | rmw_get_subscriber_names_and_types_by_node( 30 | const rmw_node_t * node, 31 | rcutils_allocator_t * allocator, 32 | const char * node_name, 33 | const char * node_namespace, 34 | bool no_demangle, 35 | rmw_names_and_types_t * topic_names_and_types) 36 | { 37 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 38 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 39 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_name, RMW_RET_ERROR); 40 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_namespace, RMW_RET_ERROR); 41 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RMW_RET_ERROR); 42 | (void) no_demangle; 43 | 44 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 45 | rmw_get_subscriber_names_and_types_by_node 46 | : node, node->implementation_identifier, 47 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 48 | 49 | rmw_ret_t rmw_ret = rmw_names_and_types_check_zero(topic_names_and_types); 50 | if (rmw_ret != RMW_RET_OK) { 51 | return rmw_ret; 52 | } 53 | 54 | auto iceoryx_topic_names_and_types = 55 | rmw_iceoryx_cpp::get_subscription_names_and_types_of_node(node_name, node_namespace); 56 | 57 | return rmw_iceoryx_cpp::fill_rmw_names_and_types( 58 | topic_names_and_types, iceoryx_topic_names_and_types, allocator); 59 | } 60 | 61 | rmw_ret_t 62 | rmw_get_publisher_names_and_types_by_node( 63 | const rmw_node_t * node, 64 | rcutils_allocator_t * allocator, 65 | const char * node_name, 66 | const char * node_namespace, 67 | bool no_demangle, 68 | rmw_names_and_types_t * topic_names_and_types) 69 | { 70 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 71 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 72 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_name, RMW_RET_ERROR); 73 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_namespace, RMW_RET_ERROR); 74 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RMW_RET_ERROR); 75 | (void) no_demangle; 76 | 77 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 78 | rmw_get_publisher_names_and_types_by_node 79 | : node, node->implementation_identifier, 80 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 81 | 82 | rmw_ret_t rmw_ret = rmw_names_and_types_check_zero(topic_names_and_types); 83 | if (rmw_ret != RMW_RET_OK) { 84 | return rmw_ret; 85 | } 86 | 87 | auto iceoryx_topic_names_and_types = 88 | rmw_iceoryx_cpp::get_publisher_names_and_types_of_node(node_name, node_namespace); 89 | 90 | return rmw_iceoryx_cpp::fill_rmw_names_and_types( 91 | topic_names_and_types, iceoryx_topic_names_and_types, allocator); 92 | } 93 | 94 | rmw_ret_t 95 | rmw_get_service_names_and_types_by_node( 96 | const rmw_node_t * node, 97 | rcutils_allocator_t * allocator, 98 | const char * node_name, 99 | const char * node_namespace, 100 | rmw_names_and_types_t * service_names_and_types) 101 | { 102 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 103 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 104 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_name, RMW_RET_ERROR); 105 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_namespace, RMW_RET_ERROR); 106 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_ERROR); 107 | 108 | /// @todo poehnl: implementation 109 | 110 | return RMW_RET_OK; 111 | } 112 | 113 | rmw_ret_t 114 | rmw_get_client_names_and_types_by_node( 115 | const rmw_node_t * node, 116 | rcutils_allocator_t * allocator, 117 | const char * node_name, 118 | const char * node_namespace, 119 | rmw_names_and_types_t * service_names_and_types) 120 | { 121 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 122 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 123 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_name, RMW_RET_ERROR); 124 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_namespace, RMW_RET_ERROR); 125 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_ERROR); 126 | 127 | /// @todo poehnl: implementation 128 | 129 | return RMW_RET_OK; 130 | } 131 | } // extern "C" 132 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_node_names.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | #include 18 | 19 | #include "iceoryx_posh/popo/untyped_subscriber.hpp" 20 | #include "iceoryx_posh/roudi/introspection_types.hpp" 21 | 22 | #include "rcutils/error_handling.h" 23 | #include "rcutils/logging_macros.h" 24 | #include "rcutils/strdup.h" 25 | 26 | #include "rmw/convert_rcutils_ret_to_rmw_ret.h" 27 | #include "rmw/impl/cpp/macros.hpp" 28 | #include "rmw/rmw.h" 29 | 30 | extern "C" 31 | { 32 | rmw_ret_t 33 | rmw_get_node_names( 34 | const rmw_node_t * node, 35 | rcutils_string_array_t * node_names, 36 | rcutils_string_array_t * node_namespaces) 37 | { 38 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 39 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_names, RMW_RET_ERROR); 40 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_namespaces, RMW_RET_ERROR); 41 | 42 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 43 | rmw_get_node_names 44 | : node, node->implementation_identifier, 45 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 46 | 47 | static iox::popo::UntypedSubscriber process_receiver(iox::roudi::IntrospectionProcessService, 48 | iox::popo::SubscriberOptions{1U, 1U, "", true}); 49 | static std::set node_names_set; 50 | 51 | bool updated = false; 52 | if (iox::SubscribeState::SUBSCRIBED != process_receiver.getSubscriptionState()) { 53 | process_receiver.subscribe(); 54 | // wait for delivery on subscribe 55 | while (!process_receiver.hasData()) { 56 | std::this_thread::sleep_for(std::chrono::milliseconds(50)); 57 | } 58 | updated = true; 59 | } else { 60 | updated = process_receiver.hasData(); 61 | } 62 | 63 | if (updated) { 64 | const void * previous_user_payload = nullptr; 65 | 66 | while (process_receiver.take() 67 | .and_then( 68 | [&](const void * userPayload) { 69 | if (previous_user_payload) { 70 | process_receiver.release(previous_user_payload); 71 | } 72 | previous_user_payload = userPayload; 73 | }) 74 | .or_else( 75 | [](auto & result) { 76 | if (result != iox::popo::ChunkReceiveResult::NO_CHUNK_AVAILABLE) { 77 | RMW_SET_ERROR_MSG("failed to take message"); 78 | } 79 | })) 80 | { 81 | } 82 | 83 | if (previous_user_payload) { 84 | const iox::roudi::ProcessIntrospectionFieldTopic * process_sample = 85 | static_cast(previous_user_payload); 86 | 87 | node_names_set.clear(); 88 | for (auto & process : process_sample->m_processList) { 89 | for (auto & runnable : process.m_nodes) { 90 | node_names_set.insert(std::string(runnable.c_str())); 91 | } 92 | } 93 | process_receiver.release(previous_user_payload); 94 | } 95 | } 96 | 97 | rcutils_allocator_t allocator = rcutils_get_default_allocator(); 98 | 99 | rcutils_ret_t rcutils_ret = 100 | rcutils_string_array_init(node_names, node_names_set.size(), &allocator); 101 | if (rcutils_ret != RCUTILS_RET_OK) { 102 | RMW_SET_ERROR_MSG(rcutils_get_error_string().str); 103 | return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); 104 | } 105 | 106 | rcutils_ret = rcutils_string_array_init(node_namespaces, node_names_set.size(), &allocator); 107 | if (rcutils_ret != RCUTILS_RET_OK) { 108 | RMW_SET_ERROR_MSG(rcutils_get_error_string().str); 109 | return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); 110 | } 111 | 112 | int i = 0; 113 | for (auto name : node_names_set) { 114 | auto pos = name.rfind("/"); 115 | 116 | auto node_name = name.substr(pos + 1, name.size()); 117 | auto node_namespace = name.substr(0, pos); 118 | 119 | node_names->data[i] = rcutils_strdup(node_name.c_str(), allocator); 120 | if (!node_names->data[i]) { 121 | RMW_SET_ERROR_MSG("could not allocate memory for node name"); 122 | goto fail; 123 | } 124 | 125 | node_namespaces->data[i] = rcutils_strdup(node_namespace.c_str(), allocator); 126 | if (!node_namespaces->data[i]) { 127 | RMW_SET_ERROR_MSG("could not allocate memory for node namespace"); 128 | goto fail; 129 | } 130 | ++i; 131 | } 132 | 133 | return RMW_RET_OK; 134 | 135 | fail: 136 | if (node_names) { 137 | rcutils_ret = rcutils_string_array_fini(node_names); 138 | if (rcutils_ret != RCUTILS_RET_OK) { 139 | RCUTILS_LOG_ERROR_NAMED( 140 | "rmw_iceoryx_cpp", 141 | "failed to cleanup during error handling: %s", rcutils_get_error_string().str); 142 | rcutils_reset_error(); 143 | } 144 | } 145 | if (node_namespaces) { 146 | rcutils_ret = rcutils_string_array_fini(node_namespaces); 147 | if (rcutils_ret != RCUTILS_RET_OK) { 148 | RCUTILS_LOG_ERROR_NAMED( 149 | "rmw_connext_cpp", 150 | "failed to cleanup during error handling: %s", rcutils_get_error_string().str); 151 | rcutils_reset_error(); 152 | } 153 | } 154 | return RMW_RET_BAD_ALLOC; 155 | } 156 | 157 | rmw_ret_t 158 | rmw_get_node_names_with_enclaves( 159 | const rmw_node_t * node, 160 | rcutils_string_array_t * node_names, 161 | rcutils_string_array_t * node_namespaces, 162 | rcutils_string_array_t * enclaves) 163 | { 164 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 165 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_names, RMW_RET_ERROR); 166 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_namespaces, RMW_RET_ERROR); 167 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(enclaves, RMW_RET_ERROR); 168 | 169 | RMW_SET_ERROR_MSG("rmw_get_node_names_with_security_contexts is not supported in iceoryx"); 170 | return RMW_RET_UNSUPPORTED; 171 | } 172 | } // extern "C" 173 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_publisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include "iceoryx_posh/capro/service_description.hpp" 19 | 20 | #include "rcutils/error_handling.h" 21 | 22 | #include "rmw/allocators.h" 23 | #include "rmw/impl/cpp/macros.hpp" 24 | 25 | #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" 26 | 27 | #include "./types/iceoryx_publisher.hpp" 28 | 29 | extern "C" 30 | { 31 | rmw_ret_t 32 | rmw_init_publisher_allocation( 33 | const rosidl_message_type_support_t * type_support, 34 | const rosidl_runtime_c__Sequence__bound * message_bounds, 35 | rmw_publisher_allocation_t * allocation) 36 | { 37 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_support, RMW_RET_ERROR); 38 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(message_bounds, RMW_RET_ERROR); 39 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocation, RMW_RET_ERROR); 40 | 41 | RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support publisher allocations."); 42 | return RMW_RET_UNSUPPORTED; 43 | } 44 | 45 | rmw_ret_t 46 | rmw_fini_publisher_allocation(rmw_publisher_allocation_t * allocation) 47 | { 48 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocation, RMW_RET_ERROR); 49 | 50 | RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support publisher allocations."); 51 | return RMW_RET_UNSUPPORTED; 52 | } 53 | 54 | rmw_publisher_t * 55 | rmw_create_publisher( 56 | const rmw_node_t * node, 57 | const rosidl_message_type_support_t * type_supports, 58 | const char * topic_name, 59 | const rmw_qos_profile_t * qos_policies, 60 | const rmw_publisher_options_t * publisher_options) 61 | { 62 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, nullptr); 63 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); 64 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); 65 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); 66 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); 67 | 68 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 69 | rmw_create_publisher 70 | : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); 71 | 72 | // create the iceoryx service description for a sender 73 | auto service_description = 74 | rmw_iceoryx_cpp::get_iceoryx_service_description(topic_name, type_supports); 75 | 76 | std::string node_full_name = std::string(node->namespace_) + std::string(node->name); 77 | rmw_publisher_t * rmw_publisher = nullptr; 78 | iox::popo::UntypedPublisher * iceoryx_sender = nullptr; 79 | IceoryxPublisher * iceoryx_publisher = nullptr; 80 | 81 | // allocate rmw_publisher 82 | rmw_publisher = rmw_publisher_allocate(); 83 | if (!rmw_publisher) { 84 | RMW_SET_ERROR_MSG("failed to allocate rmw_publisher_t"); 85 | goto fail; 86 | } 87 | 88 | // allocate iceoryx_sender 89 | iceoryx_sender = 90 | static_cast(rmw_allocate( 91 | sizeof(iox::popo::UntypedPublisher))); 92 | if (!iceoryx_sender) { 93 | RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx sender"); 94 | goto fail; 95 | } 96 | 97 | RMW_TRY_PLACEMENT_NEW( 98 | iceoryx_sender, iceoryx_sender, 99 | goto fail, iox::popo::UntypedPublisher, service_description, 100 | iox::popo::PublisherOptions{ 101 | 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); 102 | 103 | iceoryx_sender->offer(); // make the sender visible 104 | 105 | // allocate iceoryx_publisher 106 | iceoryx_publisher = 107 | static_cast(rmw_allocate(sizeof(IceoryxPublisher))); 108 | if (!iceoryx_publisher) { 109 | RMW_SET_ERROR_MSG("failed to allocate memory for rmw iceoryx publisher"); 110 | goto fail; 111 | } 112 | RMW_TRY_PLACEMENT_NEW( 113 | iceoryx_publisher, iceoryx_publisher, 114 | goto fail, IceoryxPublisher, type_supports, iceoryx_sender); 115 | 116 | // compose rmw_publisher 117 | rmw_publisher->implementation_identifier = rmw_get_implementation_identifier(); 118 | rmw_publisher->data = iceoryx_publisher; 119 | rmw_publisher->topic_name = 120 | static_cast(rmw_allocate(sizeof(char) * strlen(topic_name) + 1)); 121 | if (!rmw_publisher->topic_name) { 122 | RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); 123 | goto fail; 124 | } 125 | memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); 126 | rmw_publisher->can_loan_messages = iceoryx_publisher->is_fixed_size_; 127 | 128 | return rmw_publisher; 129 | 130 | fail: 131 | if (rmw_publisher) { 132 | if (iceoryx_sender) { 133 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 134 | iceoryx_sender->~UntypedPublisher(), iox::popo::UntypedPublisher) 135 | rmw_free(iceoryx_sender); 136 | } 137 | if (iceoryx_publisher) { 138 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 139 | iceoryx_publisher->~IceoryxPublisher(), IceoryxPublisher) 140 | rmw_free(iceoryx_publisher); 141 | } 142 | if (rmw_publisher->topic_name) { 143 | rmw_free(const_cast(rmw_publisher->topic_name)); 144 | } 145 | rmw_publisher_free(rmw_publisher); 146 | } 147 | 148 | return nullptr; 149 | } 150 | 151 | rmw_ret_t 152 | rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) 153 | { 154 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); 155 | 156 | RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support liveliness."); 157 | return RMW_RET_UNSUPPORTED; 158 | } 159 | 160 | rmw_ret_t 161 | rmw_publisher_wait_for_all_acked(const rmw_publisher_t * publisher, rmw_time_t wait_timeout) 162 | { 163 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); 164 | (void)wait_timeout; 165 | 166 | RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support wait for all acked."); 167 | return RMW_RET_UNSUPPORTED; 168 | } 169 | 170 | rmw_ret_t 171 | rmw_publisher_get_actual_qos(const rmw_publisher_t * publisher, rmw_qos_profile_t * qos) 172 | { 173 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); 174 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_ERROR); 175 | 176 | (void)publisher; 177 | 178 | /// @todo poehnl: check in detail 179 | *qos = rmw_qos_profile_default; 180 | 181 | return RMW_RET_OK; 182 | } 183 | 184 | rmw_ret_t 185 | rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) 186 | { 187 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 188 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); 189 | 190 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 191 | rmw_destroy_publisher 192 | : publisher, publisher->implementation_identifier, 193 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 194 | 195 | rmw_ret_t result = RMW_RET_OK; 196 | 197 | IceoryxPublisher * iceoryx_publisher = static_cast(publisher->data); 198 | if (iceoryx_publisher) { 199 | if (iceoryx_publisher->iceoryx_sender_) { 200 | RMW_TRY_DESTRUCTOR( 201 | iceoryx_publisher->iceoryx_sender_->~UntypedPublisher(), 202 | iceoryx_publisher->iceoryx_sender_, 203 | result = RMW_RET_ERROR) 204 | rmw_free(iceoryx_publisher->iceoryx_sender_); 205 | } 206 | RMW_TRY_DESTRUCTOR( 207 | iceoryx_publisher->~IceoryxPublisher(), 208 | iceoryx_publisher, 209 | result = RMW_RET_ERROR) 210 | rmw_free(iceoryx_publisher); 211 | } 212 | publisher->data = nullptr; 213 | 214 | rmw_free(const_cast(publisher->topic_name)); 215 | publisher->topic_name = nullptr; 216 | 217 | rmw_publisher_free(publisher); 218 | 219 | return result; 220 | } 221 | } // extern "C" 222 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_qos.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw/qos_profiles.h" 16 | #include "rcutils/error_handling.h" 17 | #include "rmw/impl/cpp/macros.hpp" 18 | 19 | extern "C" 20 | { 21 | rmw_ret_t 22 | rmw_qos_profile_check_compatible( 23 | const rmw_qos_profile_t publisher_profile, 24 | const rmw_qos_profile_t subscription_profile, 25 | rmw_qos_compatibility_type_t * compatibility, 26 | char * reason, 27 | size_t reason_size) 28 | { 29 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(compatibility, RMW_RET_INVALID_ARGUMENT); 30 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(reason, RMW_RET_INVALID_ARGUMENT); 31 | (void)publisher_profile; 32 | (void)subscription_profile; 33 | (void)reason_size; 34 | 35 | // As iceoryx does not consider QoS for matching, always set ok 36 | *compatibility = RMW_QOS_COMPATIBILITY_OK; 37 | // Un-terminated char array leads to crashes in rqt_graph 38 | reason[0] = '\0'; 39 | return RMW_RET_OK; 40 | } 41 | } // extern "C" 42 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_request.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "rcutils/error_handling.h" 17 | 18 | #include "rmw/impl/cpp/macros.hpp" 19 | #include "rmw/rmw.h" 20 | 21 | #include "./types/iceoryx_client.hpp" 22 | #include "./types/iceoryx_server.hpp" 23 | 24 | #include "rmw_iceoryx_cpp/iceoryx_serialize.hpp" 25 | #include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" 26 | 27 | extern "C" 28 | { 29 | rmw_ret_t 30 | rmw_send_request( 31 | const rmw_client_t * client, 32 | const void * ros_request, 33 | int64_t * sequence_id) 34 | { 35 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR); 36 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_ERROR); 37 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_ERROR); 38 | 39 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 40 | rmw_send_request 41 | : client, 42 | client->implementation_identifier, 43 | rmw_get_implementation_identifier(), 44 | return RMW_RET_ERROR); 45 | 46 | rmw_ret_t ret = RMW_RET_ERROR; 47 | 48 | auto iceoryx_client_abstraction = static_cast(client->data); 49 | if (!iceoryx_client_abstraction) { 50 | RMW_SET_ERROR_MSG("client data is null"); 51 | ret = RMW_RET_ERROR; 52 | return ret; 53 | } 54 | 55 | auto iceoryx_client = iceoryx_client_abstraction->iceoryx_client_; 56 | if (!iceoryx_client) { 57 | RMW_SET_ERROR_MSG("iceoryx_client is null"); 58 | ret = RMW_RET_ERROR; 59 | return ret; 60 | } 61 | 62 | iceoryx_client->loan( 63 | iceoryx_client_abstraction->request_size_, 64 | iox::CHUNK_DEFAULT_USER_PAYLOAD_ALIGNMENT) 65 | .and_then( 66 | [&](void * requestPayload) { 67 | auto requestHeader = iox::popo::RequestHeader::fromPayload(requestPayload); 68 | 69 | requestHeader->setSequenceId(iceoryx_client_abstraction->sequence_id_); 70 | *sequence_id = iceoryx_client_abstraction->sequence_id_; 71 | iceoryx_client_abstraction->sequence_id_ += 1; 72 | 73 | if (iceoryx_client_abstraction->is_fixed_size_) { 74 | memcpy(requestPayload, ros_request, iceoryx_client_abstraction->request_size_); 75 | } else { 76 | // message is not fixed size, so we have to serialize 77 | std::vector payload_vector{}; 78 | rmw_iceoryx_cpp::serializeRequest( 79 | ros_request, &iceoryx_client_abstraction->type_supports_, 80 | payload_vector); 81 | memcpy(requestPayload, payload_vector.data(), payload_vector.size()); 82 | } 83 | iceoryx_client->send(requestPayload).and_then( 84 | [&] { 85 | ret = RMW_RET_OK; 86 | }).or_else( 87 | [&](auto &) { 88 | RMW_SET_ERROR_MSG("rmw_send_request error!"); 89 | ret = RMW_RET_ERROR; 90 | }); 91 | }) 92 | .or_else( 93 | [&](auto &) { 94 | RMW_SET_ERROR_MSG("rmw_send_request error!"); 95 | ret = RMW_RET_ERROR; 96 | }); 97 | 98 | return ret; 99 | } 100 | 101 | rmw_ret_t 102 | rmw_take_request( 103 | const rmw_service_t * service, 104 | rmw_service_info_t * request_header, 105 | void * ros_request, 106 | bool * taken) 107 | { 108 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_ERROR); 109 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_ERROR); 110 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_ERROR); 111 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_ERROR); 112 | 113 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 114 | rmw_take_request 115 | : service, service->implementation_identifier, 116 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 117 | 118 | auto iceoryx_server_abstraction = static_cast(service->data); 119 | if (!iceoryx_server_abstraction) { 120 | RMW_SET_ERROR_MSG("service data is null"); 121 | return RMW_RET_ERROR; 122 | } 123 | 124 | auto iceoryx_server = iceoryx_server_abstraction->iceoryx_server_; 125 | if (!iceoryx_server) { 126 | RMW_SET_ERROR_MSG("iceoryx_server is null"); 127 | return RMW_RET_ERROR; 128 | } 129 | 130 | // this should never happen if checked already at rmw_create_service 131 | if (!rmw_iceoryx_cpp::iceoryx_is_valid_type_support(&iceoryx_server_abstraction->type_supports_)) 132 | { 133 | RMW_SET_ERROR_MSG("Use either C typesupport or CPP typesupport"); 134 | return RMW_RET_ERROR; 135 | } 136 | 137 | rmw_ret_t ret = RMW_RET_ERROR; 138 | 139 | iceoryx_server->take() 140 | .and_then( 141 | [&](const void * iceoryx_request_payload) { 142 | const auto * chunk_header = 143 | iox::mepoo::ChunkHeader::fromUserPayload( 144 | iceoryx_request_payload); 145 | const auto * iceoryx_request_header = 146 | iox::popo::RequestHeader::fromPayload( 147 | iceoryx_request_payload); 148 | 149 | request_header->source_timestamp = 0; // Unsupported until needed 150 | ret = rcutils_system_time_now(&request_header->received_timestamp); 151 | if (ret != RMW_RET_OK) { 152 | return; 153 | } 154 | request_header->request_id.sequence_number = iceoryx_request_header->getSequenceId(); 155 | auto typed_guid = iceoryx_server->getUid(); 156 | iox::popo::UniquePortId::value_type guid = 157 | static_cast(typed_guid); 158 | size_t size = sizeof(guid); 159 | auto max_rmw_storage = sizeof(request_header->request_id.writer_guid); 160 | if (!typed_guid.isValid() || size > max_rmw_storage) { 161 | RMW_SET_ERROR_MSG("Could not write server guid"); 162 | ret = RMW_RET_ERROR; 163 | return; 164 | } 165 | memcpy(request_header->request_id.writer_guid, &guid, size); 166 | 167 | // if fixed size, we fetch the data via memcpy 168 | if (iceoryx_server_abstraction->is_fixed_size_) { 169 | memcpy(ros_request, iceoryx_request_payload, chunk_header->userPayloadSize()); 170 | } else { 171 | rmw_iceoryx_cpp::deserializeRequest( 172 | static_cast(iceoryx_request_payload), 173 | &iceoryx_server_abstraction->type_supports_, 174 | ros_request); 175 | } 176 | 177 | // Hold the loaned request till we send the response in 'rmw_send_response' 178 | auto result = 179 | iceoryx_server_abstraction->request_payload_map_.insert( 180 | {request_header->request_id. 181 | sequence_number, iceoryx_request_payload}); 182 | 183 | if (result.second == false) { 184 | *taken = false; 185 | RMW_SET_ERROR_MSG("rmw_take_request: Could not store the sample pointer in the map!"); 186 | ret = RMW_RET_ERROR; 187 | return; 188 | } 189 | 190 | *taken = true; 191 | ret = RMW_RET_OK; 192 | }) 193 | .or_else( 194 | [&](auto &) { 195 | *taken = false; 196 | RMW_SET_ERROR_MSG("rmw_take_request: Taking the sample failed!"); 197 | ret = RMW_RET_ERROR; 198 | }); 199 | 200 | return ret; 201 | } 202 | } // extern "C" 203 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_response.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2022 - 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "rcutils/error_handling.h" 17 | 18 | #include "rmw/impl/cpp/macros.hpp" 19 | #include "rmw/rmw.h" 20 | 21 | #include "rmw_iceoryx_cpp/iceoryx_serialize.hpp" 22 | #include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" 23 | 24 | #include "./types/iceoryx_client.hpp" 25 | #include "./types/iceoryx_server.hpp" 26 | 27 | extern "C" 28 | { 29 | rmw_ret_t 30 | rmw_take_response( 31 | const rmw_client_t * client, 32 | rmw_service_info_t * request_header, 33 | void * ros_response, 34 | bool * taken) 35 | { 36 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); 37 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); 38 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); 39 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); 40 | 41 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 42 | rmw_take_response 43 | : client, 44 | client->implementation_identifier, 45 | rmw_get_implementation_identifier(), 46 | return RMW_RET_ERROR); 47 | 48 | auto iceoryx_client_abstraction = static_cast(client->data); 49 | if (!iceoryx_client_abstraction) { 50 | RMW_SET_ERROR_MSG("client data is null"); 51 | return RMW_RET_ERROR; 52 | } 53 | 54 | auto iceoryx_client = iceoryx_client_abstraction->iceoryx_client_; 55 | if (!iceoryx_client) { 56 | RMW_SET_ERROR_MSG("iceoryx_client is null"); 57 | return RMW_RET_ERROR; 58 | } 59 | 60 | rmw_ret_t ret = RMW_RET_ERROR; 61 | 62 | iceoryx_client->take() 63 | .and_then( 64 | [&](const void * iceoryx_response_payload) { 65 | auto iceoryx_response_header = iox::popo::ResponseHeader::fromPayload( 66 | iceoryx_response_payload); 67 | 68 | if (iceoryx_response_header->getSequenceId() == 69 | iceoryx_client_abstraction->sequence_id_ - 1) 70 | { 71 | const iox::mepoo::ChunkHeader * chunk_header = 72 | iox::mepoo::ChunkHeader::fromUserPayload(iceoryx_response_payload); 73 | 74 | auto typed_guid = chunk_header->originId(); 75 | iox::popo::UniquePortId::value_type guid = 76 | static_cast(typed_guid); 77 | size_t size = sizeof(guid); 78 | auto max_rmw_storage = sizeof(request_header->request_id.writer_guid); 79 | if (!typed_guid.isValid() || size > max_rmw_storage) { 80 | RMW_SET_ERROR_MSG("Could not write server guid"); 81 | ret = RMW_RET_ERROR; 82 | return; 83 | } 84 | memcpy(request_header->request_id.writer_guid, &guid, size); 85 | request_header->request_id.sequence_number = iceoryx_response_header->getSequenceId(); 86 | request_header->source_timestamp = 0; // Unsupported until needed 87 | ret = rcutils_system_time_now(&request_header->received_timestamp); 88 | if (ret != RMW_RET_OK) { 89 | return; 90 | } 91 | 92 | // if fixed size, we fetch the data via memcpy 93 | if (iceoryx_client_abstraction->is_fixed_size_) { 94 | memcpy(ros_response, iceoryx_response_payload, chunk_header->userPayloadSize()); 95 | } else { 96 | rmw_iceoryx_cpp::deserializeResponse( 97 | static_cast(iceoryx_response_payload), 98 | &iceoryx_client_abstraction->type_supports_, 99 | ros_response); 100 | } 101 | 102 | *taken = true; 103 | ret = RMW_RET_OK; 104 | } else { 105 | RMW_SET_ERROR_MSG("Got response with outdated sequence number!"); 106 | *taken = false; 107 | ret = RMW_RET_ERROR; 108 | } 109 | iceoryx_client->releaseResponse(iceoryx_response_payload); 110 | }) 111 | .or_else( 112 | [&](auto &) { 113 | *taken = false; 114 | RMW_SET_ERROR_MSG("No chunk in iceoryx_client"); 115 | ret = RMW_RET_ERROR; 116 | }); 117 | 118 | return ret; 119 | } 120 | 121 | rmw_ret_t 122 | rmw_send_response( 123 | const rmw_service_t * service, 124 | rmw_request_id_t * request_header, 125 | void * ros_response) 126 | { 127 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); 128 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); 129 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); 130 | 131 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 132 | rmw_send_response 133 | : service, service->implementation_identifier, 134 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 135 | 136 | auto iceoryx_server_abstraction = static_cast(service->data); 137 | if (!iceoryx_server_abstraction) { 138 | RMW_SET_ERROR_MSG("service data is null"); 139 | return RMW_RET_ERROR; 140 | } 141 | 142 | auto iceoryx_server = iceoryx_server_abstraction->iceoryx_server_; 143 | if (!iceoryx_server) { 144 | RMW_SET_ERROR_MSG("iceoryx_server is null"); 145 | return RMW_RET_ERROR; 146 | } 147 | 148 | rmw_ret_t ret = RMW_RET_ERROR; 149 | auto & payload_ptr_map = iceoryx_server_abstraction->request_payload_map_; 150 | 151 | if (payload_ptr_map.empty()) { 152 | RMW_SET_ERROR_MSG("'rmw_take_request' needs to be called before 'rmw_send_response'!"); 153 | ret = RMW_RET_ERROR; 154 | return ret; 155 | } 156 | 157 | auto it = payload_ptr_map.find(request_header->sequence_number); 158 | 159 | if (it == payload_ptr_map.end()) { 160 | RMW_SET_ERROR_MSG("Could not find the payload pointer in the map"); 161 | ret = RMW_RET_ERROR; 162 | return ret; 163 | } 164 | 165 | auto * iceoryx_request_header = iox::popo::RequestHeader::fromPayload((*it).second); 166 | 167 | iceoryx_server->loan( 168 | iceoryx_request_header, iceoryx_server_abstraction->response_size_, 169 | iox::CHUNK_DEFAULT_USER_PAYLOAD_ALIGNMENT) 170 | .and_then( 171 | [&](void * responsePayload) { 172 | if (iceoryx_server_abstraction->is_fixed_size_) { 173 | memcpy(responsePayload, ros_response, iceoryx_server_abstraction->response_size_); 174 | } else { 175 | // message is not fixed size, so we have to serialize 176 | std::vector payload_vector{}; 177 | rmw_iceoryx_cpp::serializeResponse( 178 | ros_response, 179 | &iceoryx_server_abstraction->type_supports_, payload_vector); 180 | memcpy(responsePayload, payload_vector.data(), payload_vector.size()); 181 | } 182 | iceoryx_server->send(responsePayload).and_then( 183 | [&] { 184 | ret = RMW_RET_OK; 185 | }).or_else( 186 | [&](auto &) { 187 | RMW_SET_ERROR_MSG("rmw_send_response send error!"); 188 | ret = RMW_RET_ERROR; 189 | }); 190 | }) 191 | .or_else( 192 | [&](auto &) { 193 | RMW_SET_ERROR_MSG("rmw_send_response loan error!"); 194 | ret = RMW_RET_ERROR; 195 | }); 196 | 197 | // Release the hold request and delete the entry from the map 198 | iceoryx_server->releaseRequest((*it).second); 199 | payload_ptr_map.erase(request_header->sequence_number); 200 | 201 | return ret; 202 | } 203 | } // extern "C" 204 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_serialize.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "rcutils/error_handling.h" 18 | 19 | #include "rmw/rmw.h" 20 | 21 | #include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" 22 | #include "rmw_iceoryx_cpp/iceoryx_serialize.hpp" 23 | #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" 24 | 25 | #include "rosidl_typesupport_introspection_c/identifier.h" 26 | 27 | #include "rosidl_typesupport_introspection_cpp/identifier.hpp" 28 | 29 | namespace details 30 | { 31 | rmw_ret_t 32 | copy_payload_into_serialized_message( 33 | rmw_serialized_message_t * serialized_message, 34 | const void * payload, 35 | size_t payload_size) 36 | { 37 | auto ret = rmw_serialized_message_resize(serialized_message, payload_size); 38 | if (RMW_RET_OK == ret) { 39 | memcpy(serialized_message->buffer, payload, payload_size); 40 | serialized_message->buffer_length = payload_size; 41 | } 42 | return ret; 43 | } 44 | } // namespace details 45 | 46 | extern "C" 47 | { 48 | rmw_ret_t 49 | rmw_serialize( 50 | const void * ros_message, 51 | const rosidl_message_type_support_t * type_supports, 52 | rmw_serialized_message_t * serialized_message) 53 | { 54 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_message, RMW_RET_ERROR); 55 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_supports, RMW_RET_ERROR); 56 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(serialized_message, RMW_RET_ERROR); 57 | 58 | // it's a fixed size message, so we memcpy 59 | if (rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)) { 60 | return details::copy_payload_into_serialized_message( 61 | serialized_message, ros_message, rmw_iceoryx_cpp::iceoryx_get_message_size(type_supports)); 62 | } 63 | 64 | // it's no fixed size message, so we have to serialize 65 | std::vector payload_vector{}; 66 | rmw_iceoryx_cpp::serialize(ros_message, type_supports, payload_vector); 67 | 68 | return details::copy_payload_into_serialized_message( 69 | serialized_message, payload_vector.data(), payload_vector.size()); 70 | } 71 | 72 | rmw_ret_t 73 | rmw_deserialize( 74 | const rmw_serialized_message_t * serialized_message, 75 | const rosidl_message_type_support_t * type_supports, 76 | void * ros_message) 77 | { 78 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(serialized_message, RMW_RET_ERROR); 79 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_supports, RMW_RET_ERROR); 80 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(ros_message, RMW_RET_ERROR); 81 | 82 | // it's a fixed size message, so we memcpy 83 | if (rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)) { 84 | memcpy(ros_message, serialized_message->buffer, serialized_message->buffer_length); 85 | return RMW_RET_OK; 86 | } 87 | 88 | rmw_iceoryx_cpp::deserialize( 89 | reinterpret_cast(serialized_message->buffer), type_supports, ros_message); 90 | 91 | return RMW_RET_OK; 92 | } 93 | 94 | rmw_ret_t 95 | rmw_get_serialized_message_size( 96 | const rosidl_message_type_support_t * type_supports, 97 | const rosidl_runtime_c__Sequence__bound * message_bounds, 98 | size_t * size) 99 | { 100 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_supports, RMW_RET_ERROR); 101 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(message_bounds, RMW_RET_ERROR); 102 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(size, RMW_RET_ERROR); 103 | 104 | *size = rmw_iceoryx_cpp::iceoryx_get_message_size(type_supports); 105 | 106 | return RMW_RET_OK; 107 | } 108 | } // extern "C" 109 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_service.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2022 - 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "rcutils/error_handling.h" 17 | 18 | #include "rmw/allocators.h" 19 | #include "rmw/impl/cpp/macros.hpp" 20 | #include "rmw/rmw.h" 21 | 22 | #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" 23 | 24 | #include "types/iceoryx_server.hpp" 25 | 26 | extern "C" 27 | { 28 | rmw_service_t * 29 | rmw_create_service( 30 | const rmw_node_t * node, 31 | const rosidl_service_type_support_t * type_supports, 32 | const char * service_name, 33 | const rmw_qos_profile_t * qos_policies) 34 | { 35 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, nullptr); 36 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); 37 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr); 38 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); 39 | 40 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 41 | rmw_create_service 42 | : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); 43 | 44 | // create the iceoryx service description for a sender 45 | auto service_description = 46 | rmw_iceoryx_cpp::get_iceoryx_service_description(service_name, type_supports); 47 | 48 | std::string node_full_name = std::string(node->namespace_) + std::string(node->name); 49 | rmw_service_t * rmw_service = nullptr; 50 | iox::popo::UntypedServer * iceoryx_server = nullptr; 51 | IceoryxServer * iceoryx_server_abstraction = nullptr; 52 | 53 | bool returnOnError = false; 54 | 55 | auto cleanupAfterError = [&]() { 56 | if (rmw_service) { 57 | if (iceoryx_server) { 58 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 59 | iceoryx_server->~UntypedServer(), iox::popo::UntypedServer) 60 | rmw_free(iceoryx_server); 61 | } 62 | if (iceoryx_server_abstraction) { 63 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 64 | iceoryx_server_abstraction->~IceoryxServer(), IceoryxServer) 65 | rmw_free(iceoryx_server_abstraction); 66 | } 67 | if (rmw_service->service_name) { 68 | rmw_free(const_cast(rmw_service->service_name)); 69 | } 70 | rmw_service_free(rmw_service); 71 | returnOnError = true; 72 | } 73 | }; 74 | 75 | rmw_service = rmw_service_allocate(); 76 | if (!rmw_service) { 77 | RMW_SET_ERROR_MSG("failed to allocate memory for service"); 78 | cleanupAfterError(); 79 | return nullptr; 80 | } 81 | 82 | iceoryx_server = 83 | static_cast(rmw_allocate( 84 | sizeof(iox::popo::UntypedServer))); 85 | 86 | if (!iceoryx_server) { 87 | RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx server"); 88 | cleanupAfterError(); 89 | return nullptr; 90 | } 91 | 92 | RMW_TRY_PLACEMENT_NEW( 93 | iceoryx_server, iceoryx_server, 94 | cleanupAfterError(), iox::popo::UntypedServer, service_description, 95 | iox::popo::ServerOptions{ 96 | qos_policies->depth, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); 97 | if (returnOnError) { 98 | return nullptr; 99 | } 100 | 101 | iceoryx_server->offer(); 102 | 103 | iceoryx_server_abstraction = 104 | static_cast(rmw_allocate(sizeof(IceoryxServer))); 105 | if (!iceoryx_server_abstraction) { 106 | RMW_SET_ERROR_MSG("failed to allocate memory for rmw iceoryx publisher"); 107 | cleanupAfterError(); 108 | return nullptr; 109 | } 110 | RMW_TRY_PLACEMENT_NEW( 111 | iceoryx_server_abstraction, iceoryx_server_abstraction, 112 | cleanupAfterError(), IceoryxServer, type_supports, iceoryx_server); 113 | if (returnOnError) { 114 | return nullptr; 115 | } 116 | 117 | rmw_service->implementation_identifier = rmw_get_implementation_identifier(); 118 | rmw_service->data = iceoryx_server_abstraction; 119 | 120 | rmw_service->service_name = 121 | static_cast(rmw_allocate(sizeof(char) * strlen(service_name) + 1)); 122 | if (!rmw_service->service_name) { 123 | RMW_SET_ERROR_MSG("failed to allocate memory for service name"); 124 | cleanupAfterError(); 125 | return nullptr; 126 | } 127 | memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); 128 | 129 | return rmw_service; 130 | } 131 | 132 | rmw_ret_t 133 | rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) 134 | { 135 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 136 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_ERROR); 137 | 138 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 139 | rmw_destroy_service 140 | : service, service->implementation_identifier, 141 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 142 | 143 | rmw_ret_t result = RMW_RET_OK; 144 | 145 | IceoryxServer * iceoryx_server_abstraction = static_cast(service->data); 146 | if (iceoryx_server_abstraction) { 147 | if (iceoryx_server_abstraction->iceoryx_server_) { 148 | RMW_TRY_DESTRUCTOR( 149 | iceoryx_server_abstraction->iceoryx_server_->~UntypedServer(), 150 | iceoryx_server_abstraction->iceoryx_server_, 151 | result = RMW_RET_ERROR) 152 | rmw_free(iceoryx_server_abstraction->iceoryx_server_); 153 | } 154 | RMW_TRY_DESTRUCTOR( 155 | iceoryx_server_abstraction->~IceoryxServer(), 156 | iceoryx_server_abstraction, 157 | result = RMW_RET_ERROR) 158 | rmw_free(iceoryx_server_abstraction); 159 | } 160 | 161 | service->data = nullptr; 162 | 163 | rmw_free(const_cast(service->service_name)); 164 | service->service_name = nullptr; 165 | 166 | rmw_service_free(service); 167 | 168 | return result; 169 | } 170 | 171 | rmw_ret_t rmw_service_response_publisher_get_actual_qos( 172 | const rmw_service_t * service, 173 | rmw_qos_profile_t * qos) 174 | { 175 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); 176 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); 177 | 178 | *qos = rmw_qos_profile_default; 179 | 180 | return RMW_RET_OK; 181 | } 182 | 183 | rmw_ret_t rmw_service_request_subscription_get_actual_qos( 184 | const rmw_service_t * service, 185 | rmw_qos_profile_t * qos) 186 | { 187 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); 188 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); 189 | 190 | *qos = rmw_qos_profile_default; 191 | 192 | return RMW_RET_OK; 193 | } 194 | 195 | rmw_ret_t rmw_service_set_on_new_request_callback( 196 | rmw_service_t * service, 197 | rmw_event_callback_t callback, 198 | const void * user_data) 199 | { 200 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); 201 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(callback, RMW_RET_INVALID_ARGUMENT); 202 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(user_data, RMW_RET_INVALID_ARGUMENT); 203 | 204 | return RMW_RET_UNSUPPORTED; 205 | } 206 | } // extern "C" 207 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_service_names_and_types.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "rcutils/error_handling.h" 17 | 18 | #include "rmw/get_service_names_and_types.h" 19 | #include "rmw/impl/cpp/macros.hpp" 20 | #include "rmw/names_and_types.h" 21 | #include "rmw/rmw.h" 22 | 23 | #include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp" 24 | 25 | extern "C" 26 | { 27 | rmw_ret_t 28 | rmw_get_service_names_and_types( 29 | const rmw_node_t * node, 30 | rcutils_allocator_t * allocator, 31 | rmw_names_and_types_t * service_names_and_types) 32 | { 33 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 34 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 35 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_ERROR); 36 | 37 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 38 | rmw_get_service_names_and_types 39 | : node, node->implementation_identifier, 40 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 41 | 42 | rmw_ret_t rmw_ret = rmw_names_and_types_check_zero(service_names_and_types); 43 | if (rmw_ret != RMW_RET_OK) { 44 | return rmw_ret; // error already set 45 | } 46 | 47 | auto iceoryx_service_names_and_types = rmw_iceoryx_cpp::get_service_names_and_types(); 48 | 49 | return rmw_iceoryx_cpp::fill_rmw_names_and_types( 50 | service_names_and_types, iceoryx_service_names_and_types, allocator); 51 | 52 | return RMW_RET_OK; 53 | } 54 | } // extern "C" 55 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_service_server_is_available.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "rcutils/error_handling.h" 17 | 18 | #include "rmw/impl/cpp/macros.hpp" 19 | #include "rmw/rmw.h" 20 | 21 | #include "./types/iceoryx_client.hpp" 22 | 23 | extern "C" 24 | { 25 | rmw_ret_t 26 | rmw_service_server_is_available( 27 | const rmw_node_t * node, 28 | const rmw_client_t * client, 29 | bool * is_available) 30 | { 31 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 32 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_ERROR); 33 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(is_available, RMW_RET_ERROR); 34 | 35 | rmw_ret_t ret = RMW_RET_ERROR; 36 | 37 | auto iceoryx_client_abstraction = static_cast(client->data); 38 | if (!iceoryx_client_abstraction) { 39 | RMW_SET_ERROR_MSG("client data is null"); 40 | ret = RMW_RET_ERROR; 41 | return ret; 42 | } 43 | 44 | auto iceoryx_client = iceoryx_client_abstraction->iceoryx_client_; 45 | if (!iceoryx_client) { 46 | RMW_SET_ERROR_MSG("iceoryx_client is null"); 47 | ret = RMW_RET_ERROR; 48 | return ret; 49 | } 50 | 51 | *is_available = iceoryx_client->getConnectionState() == iox::ConnectionState::CONNECTED; 52 | 53 | ret = RMW_RET_OK; 54 | return ret; 55 | } 56 | } // extern "C" 57 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_subscription.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 - 2022 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include "iceoryx_posh/capro/service_description.hpp" 19 | 20 | #include "rcutils/error_handling.h" 21 | 22 | #include "rmw/allocators.h" 23 | #include "rmw/impl/cpp/macros.hpp" 24 | #include "rmw/rmw.h" 25 | 26 | #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" 27 | 28 | #include "./types/iceoryx_subscription.hpp" 29 | 30 | extern "C" 31 | { 32 | rmw_ret_t 33 | rmw_init_subscription_allocation( 34 | const rosidl_message_type_support_t * type_supports, 35 | const rosidl_runtime_c__Sequence__bound * message_bounds, 36 | rmw_subscription_allocation_t * allocation) 37 | { 38 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_supports, RMW_RET_ERROR); 39 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(message_bounds, RMW_RET_ERROR); 40 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocation, RMW_RET_ERROR); // might be null 41 | 42 | RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support subscription allocations."); 43 | return RMW_RET_UNSUPPORTED; 44 | } 45 | 46 | rmw_ret_t 47 | rmw_fini_subscription_allocation(rmw_subscription_allocation_t * allocation) 48 | { 49 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocation, RMW_RET_ERROR); 50 | 51 | RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support subscription allocations."); 52 | return RMW_RET_UNSUPPORTED; 53 | } 54 | 55 | rmw_subscription_t * 56 | rmw_create_subscription( 57 | const rmw_node_t * node, 58 | const rosidl_message_type_support_t * type_supports, 59 | const char * topic_name, 60 | const rmw_qos_profile_t * qos_policies, 61 | const rmw_subscription_options_t * subscription_options) 62 | { 63 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, nullptr); 64 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); 65 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); 66 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); 67 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); 68 | 69 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 70 | rmw_create_subscription 71 | : node, node->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); 72 | 73 | // create the iceoryx service description for a receiver 74 | auto service_description = 75 | rmw_iceoryx_cpp::get_iceoryx_service_description(topic_name, type_supports); 76 | 77 | std::string node_full_name = std::string(node->namespace_) + std::string(node->name); 78 | rmw_subscription_t * rmw_subscription = nullptr; 79 | iox::popo::UntypedSubscriber * iceoryx_receiver = nullptr; 80 | IceoryxSubscription * iceoryx_subscription = nullptr; 81 | 82 | rmw_subscription = rmw_subscription_allocate(); 83 | if (!rmw_subscription) { 84 | RMW_SET_ERROR_MSG("failed to allocate subscription"); 85 | goto fail; 86 | } 87 | 88 | iceoryx_receiver = 89 | static_cast(rmw_allocate( 90 | sizeof(iox::popo::UntypedSubscriber))); 91 | if (!iceoryx_receiver) { 92 | RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx receiver"); 93 | goto fail; 94 | } 95 | RMW_TRY_PLACEMENT_NEW( 96 | iceoryx_receiver, iceoryx_receiver, goto fail, 97 | iox::popo::UntypedSubscriber, service_description, 98 | iox::popo::SubscriberOptions{ 99 | qos_policies->depth, 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); 100 | 101 | // instant subscribe, queue size form qos settings 102 | iceoryx_receiver->subscribe(); 103 | 104 | iceoryx_subscription = 105 | static_cast(rmw_allocate(sizeof(IceoryxSubscription))); 106 | if (!iceoryx_subscription) { 107 | RMW_SET_ERROR_MSG("failed to allocate memory for rmw iceoryx subscription"); 108 | goto fail; 109 | } 110 | RMW_TRY_PLACEMENT_NEW( 111 | iceoryx_subscription, iceoryx_subscription, 112 | goto fail, IceoryxSubscription, type_supports, iceoryx_receiver) 113 | 114 | rmw_subscription->implementation_identifier = rmw_get_implementation_identifier(); 115 | rmw_subscription->data = iceoryx_subscription; 116 | rmw_subscription->topic_name = 117 | static_cast(rmw_allocate(sizeof(char) * strlen(topic_name) + 1)); 118 | if (!rmw_subscription->topic_name) { 119 | RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); 120 | goto fail; 121 | } 122 | memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); 123 | 124 | rmw_subscription->can_loan_messages = iceoryx_subscription->is_fixed_size_; 125 | 126 | return rmw_subscription; 127 | 128 | fail: 129 | if (rmw_subscription) { 130 | if (iceoryx_receiver) { 131 | /// @todo Can we avoid to use the impl here? 132 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 133 | iceoryx_receiver->~UntypedSubscriber(), iox::popo::UntypedSubscriber) 134 | rmw_free(iceoryx_receiver); 135 | } 136 | if (iceoryx_subscription) { 137 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 138 | iceoryx_subscription->~IceoryxSubscription(), IceoryxSubscription) 139 | rmw_free(iceoryx_subscription); 140 | } 141 | if (rmw_subscription->topic_name) { 142 | rmw_free(const_cast(rmw_subscription->topic_name)); 143 | } 144 | rmw_subscription_free(rmw_subscription); 145 | } 146 | 147 | return nullptr; 148 | } 149 | 150 | rmw_ret_t 151 | rmw_subscription_get_actual_qos( 152 | const rmw_subscription_t * subscription, 153 | rmw_qos_profile_t * qos) 154 | { 155 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); 156 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_ERROR); 157 | 158 | /// @todo poehnl: check in detail 159 | *qos = rmw_qos_profile_default; 160 | 161 | return RMW_RET_OK; 162 | } 163 | 164 | rmw_ret_t rmw_subscription_set_content_filter( 165 | rmw_subscription_t * subscription, 166 | const rmw_subscription_content_filter_options_t * options) 167 | { 168 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); 169 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); 170 | 171 | return RMW_RET_UNSUPPORTED; 172 | } 173 | 174 | rmw_ret_t rmw_subscription_get_content_filter( 175 | const rmw_subscription_t * subscription, 176 | rcutils_allocator_t * allocator, 177 | rmw_subscription_content_filter_options_t * options) 178 | { 179 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); 180 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); 181 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); 182 | 183 | return RMW_RET_UNSUPPORTED; 184 | } 185 | 186 | rmw_ret_t rmw_subscription_set_on_new_message_callback( 187 | rmw_subscription_t * subscription, 188 | rmw_event_callback_t callback, 189 | const void * user_data) 190 | { 191 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); 192 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(callback, RMW_RET_INVALID_ARGUMENT); 193 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(user_data, RMW_RET_INVALID_ARGUMENT); 194 | 195 | return RMW_RET_UNSUPPORTED; 196 | } 197 | 198 | rmw_ret_t 199 | rmw_destroy_subscription( 200 | rmw_node_t * node, 201 | rmw_subscription_t * subscription) 202 | { 203 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 204 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); 205 | 206 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 207 | rmw_destroy_subscription 208 | : subscription, subscription->implementation_identifier, 209 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 210 | 211 | rmw_ret_t result = RMW_RET_OK; 212 | 213 | IceoryxSubscription * iceoryx_subscription = 214 | static_cast(subscription->data); 215 | if (iceoryx_subscription) { 216 | if (iceoryx_subscription->iceoryx_receiver_) { 217 | // @todo Can we avoid to use the impl here? 218 | RMW_TRY_DESTRUCTOR( 219 | iceoryx_subscription->iceoryx_receiver_->~UntypedSubscriber(), 220 | iceoryx_subscription->iceoryx_receiver_, 221 | result = RMW_RET_ERROR) 222 | rmw_free(iceoryx_subscription->iceoryx_receiver_); 223 | } 224 | RMW_TRY_DESTRUCTOR( 225 | iceoryx_subscription->~IceoryxSubscription(), 226 | iceoryx_subscription, 227 | result = RMW_RET_ERROR) 228 | rmw_free(iceoryx_subscription); 229 | } 230 | subscription->data = nullptr; 231 | 232 | rmw_free(const_cast(subscription->topic_name)); 233 | subscription->topic_name = nullptr; 234 | 235 | rmw_subscription_free(subscription); 236 | 237 | return result; 238 | } 239 | } // extern "C" 240 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_topic_names_and_types.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "rcutils/error_handling.h" 18 | #include "rcutils/logging_macros.h" 19 | #include "rcutils/strdup.h" 20 | #include "rcutils/types.h" 21 | 22 | #include "rmw/impl/cpp/macros.hpp" 23 | #include "rmw/get_topic_names_and_types.h" 24 | #include "rmw/names_and_types.h" 25 | #include "rmw/rmw.h" 26 | 27 | #include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp" 28 | 29 | extern "C" 30 | { 31 | rmw_ret_t 32 | rmw_get_topic_names_and_types( 33 | const rmw_node_t * node, 34 | rcutils_allocator_t * allocator, 35 | bool no_demangle, 36 | rmw_names_and_types_t * topic_names_and_types) 37 | { 38 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); 39 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); 40 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RMW_RET_ERROR); 41 | (void) no_demangle; 42 | 43 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 44 | rmw_get_topic_names_and_types 45 | : node, node->implementation_identifier, 46 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 47 | 48 | rmw_ret_t rmw_ret = rmw_names_and_types_check_zero(topic_names_and_types); 49 | if (rmw_ret != RMW_RET_OK) { 50 | return rmw_ret; // error already set 51 | } 52 | 53 | auto iceoryx_topic_names_and_types = rmw_iceoryx_cpp::get_topic_names_and_types(); 54 | 55 | return rmw_iceoryx_cpp::fill_rmw_names_and_types( 56 | topic_names_and_types, iceoryx_topic_names_and_types, allocator); 57 | } 58 | } // extern "C" 59 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_trigger_guard_condition.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "iceoryx_posh/popo/user_trigger.hpp" 17 | 18 | #include "rcutils/error_handling.h" 19 | 20 | #include "rmw/impl/cpp/macros.hpp" 21 | #include "rmw/rmw.h" 22 | 23 | extern "C" 24 | { 25 | rmw_ret_t 26 | rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition_handle) 27 | { 28 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(guard_condition_handle, RMW_RET_ERROR); 29 | 30 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 31 | rmw_trigger_guard_condition 32 | : guard_condition_handle, 33 | guard_condition_handle->implementation_identifier, 34 | rmw_get_implementation_identifier(), 35 | return RMW_RET_ERROR); 36 | 37 | auto guard_condition = static_cast(guard_condition_handle->data); 38 | if (!guard_condition) { 39 | RMW_SET_ERROR_MSG("guard condition is null"); 40 | return RMW_RET_ERROR; 41 | } 42 | guard_condition->trigger(); 43 | return RMW_RET_OK; 44 | } 45 | } // extern "C" 46 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_wait.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 - 2023 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include "iceoryx_posh/popo/untyped_subscriber.hpp" 19 | #include "iceoryx_posh/popo/wait_set.hpp" 20 | #include "iceoryx_posh/popo/user_trigger.hpp" 21 | 22 | #include "rcutils/error_handling.h" 23 | 24 | #include "rmw/impl/cpp/macros.hpp" 25 | #include "rmw/rmw.h" 26 | 27 | #include "./types/iceoryx_subscription.hpp" 28 | #include "./types/iceoryx_client.hpp" 29 | #include "./types/iceoryx_server.hpp" 30 | 31 | extern "C" 32 | { 33 | rmw_ret_t 34 | rmw_wait( 35 | rmw_subscriptions_t * subscriptions, 36 | rmw_guard_conditions_t * guard_conditions, 37 | rmw_services_t * services, 38 | rmw_clients_t * clients, 39 | rmw_events_t * events, 40 | rmw_wait_set_t * wait_set, 41 | const rmw_time_t * wait_timeout) 42 | { 43 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscriptions, RMW_RET_ERROR); 44 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(guard_conditions, RMW_RET_ERROR); 45 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(services, RMW_RET_ERROR); 46 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(clients, RMW_RET_ERROR); 47 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(events, RMW_RET_ERROR); 48 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_ERROR); 49 | 50 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 51 | rmw_wait 52 | : waitset, wait_set->implementation_identifier, 53 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 54 | 55 | iox::popo::WaitSet * waitset = 56 | static_cast *>(wait_set->data); 57 | if (!waitset) { 58 | return RMW_RET_ERROR; 59 | } 60 | 61 | bool skip_wait{false}; 62 | // attach all iceoryx subscriber to WaitSet 63 | for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { 64 | auto iceoryx_subscription = 65 | static_cast(subscriptions->subscribers[i]); 66 | auto iceoryx_receiver = iceoryx_subscription->iceoryx_receiver_; 67 | 68 | waitset->attachState(*iceoryx_receiver, iox::popo::SubscriberState::HAS_DATA).or_else( 69 | [&](auto &) { 70 | RMW_SET_ERROR_MSG("failed to attach subscriber"); 71 | skip_wait = true; 72 | }); 73 | } 74 | 75 | // attach all iceoryx servers to WaitSet 76 | for (size_t i = 0; i < services->service_count; ++i) { 77 | auto iceoryx_server_abstraction = 78 | static_cast(services->services[i]); 79 | auto iceoryx_server = iceoryx_server_abstraction->iceoryx_server_; 80 | 81 | waitset->attachState(*iceoryx_server, iox::popo::ServerState::HAS_REQUEST).or_else( 82 | [&](auto &) { 83 | RMW_SET_ERROR_MSG("failed to attach service"); 84 | skip_wait = true; 85 | }); 86 | } 87 | 88 | // attach all iceoryx client to WaitSet 89 | for (size_t i = 0; i < clients->client_count; ++i) { 90 | auto iceoryx_client_abstraction = 91 | static_cast(clients->clients[i]); 92 | auto iceoryx_client = iceoryx_client_abstraction->iceoryx_client_; 93 | 94 | waitset->attachState(*iceoryx_client, iox::popo::ClientState::HAS_RESPONSE).or_else( 95 | [&](auto &) { 96 | RMW_SET_ERROR_MSG("failed to attach client"); 97 | skip_wait = true; 98 | }); 99 | } 100 | 101 | // attach all guard conditions to WaitSet 102 | for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { 103 | auto iceoryx_guard_condition = 104 | static_cast(guard_conditions->guard_conditions[i]); 105 | 106 | waitset->attachEvent(*iceoryx_guard_condition).or_else( 107 | [&](auto) { 108 | RMW_SET_ERROR_MSG("failed to attach guard condition"); 109 | skip_wait = true; 110 | }); 111 | } 112 | 113 | if (skip_wait) { 114 | goto after_wait; 115 | } 116 | 117 | // The triggered entities are checked below individually, this could be refactored by looping 118 | // over the vector returned by 'wait()' and using 'vectorEntry->doesOriginateFrom()' 119 | if (!wait_timeout) { 120 | waitset->wait(); 121 | } else { 122 | auto sec = iox::units::Duration::fromSeconds(wait_timeout->sec); 123 | auto nsec = iox::units::Duration::fromNanoseconds(wait_timeout->nsec); 124 | auto timeout = sec + nsec; 125 | 126 | waitset->timedWait(iox::units::Duration(timeout)); 127 | } 128 | 129 | after_wait: 130 | // reset all the subscriptions that don't have new data 131 | for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { 132 | auto iceoryx_subscription = 133 | static_cast(subscriptions->subscribers[i]); 134 | iox::popo::UntypedSubscriber * iceoryx_receiver = iceoryx_subscription->iceoryx_receiver_; 135 | 136 | // remove waitset from all receivers because next call a new waitset could be provided 137 | waitset->detachState(*iceoryx_receiver, iox::popo::SubscriberState::HAS_DATA); 138 | 139 | if (!iceoryx_receiver->hasData()) { 140 | subscriptions->subscribers[i] = nullptr; 141 | } 142 | } 143 | 144 | // reset all the servers that don't have new data 145 | for (size_t i = 0; i < services->service_count; ++i) { 146 | auto iceoryx_server_abstraction = 147 | static_cast(services->services[i]); 148 | iox::popo::UntypedServer * iceoryx_server = iceoryx_server_abstraction->iceoryx_server_; 149 | 150 | // remove waitset from all receivers because next call a new waitset could be provided 151 | waitset->detachState(*iceoryx_server, iox::popo::ServerState::HAS_REQUEST); 152 | 153 | if (!iceoryx_server->hasRequests()) { 154 | services->services[i] = nullptr; 155 | } 156 | } 157 | 158 | // reset all the clients that don't have new data 159 | for (size_t i = 0; i < clients->client_count; ++i) { 160 | auto iceoryx_client_abstraction = 161 | static_cast(clients->clients[i]); 162 | iox::popo::UntypedClient * iceoryx_client = iceoryx_client_abstraction->iceoryx_client_; 163 | 164 | // remove waitset from all receivers because next call a new waitset could be provided 165 | waitset->detachState(*iceoryx_client, iox::popo::ClientState::HAS_RESPONSE); 166 | 167 | if (!iceoryx_client->hasResponses()) { 168 | clients->clients[i] = nullptr; 169 | } 170 | } 171 | 172 | // reset all the guard_conditions that have not triggered 173 | for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { 174 | auto iceoryx_guard_condition = 175 | static_cast(guard_conditions->guard_conditions[i]); 176 | 177 | waitset->detachEvent(*iceoryx_guard_condition); 178 | 179 | if (!iceoryx_guard_condition->hasTriggered()) { 180 | guard_conditions->guard_conditions[i] = nullptr; 181 | } 182 | } 183 | 184 | return RMW_RET_OK; 185 | } 186 | } // extern "C" 187 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/rmw_wait_set.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "./iceoryx_identifier.hpp" 17 | 18 | #include "iceoryx_posh/roudi/introspection_types.hpp" 19 | #include "iceoryx_posh/popo/wait_set.hpp" 20 | 21 | #include "rcutils/error_handling.h" 22 | 23 | #include "rmw/impl/cpp/macros.hpp" 24 | #include "rmw/rmw.h" 25 | 26 | extern "C" 27 | { 28 | rmw_wait_set_t * 29 | rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) 30 | { 31 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, nullptr); 32 | (void)max_conditions; 33 | 34 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 35 | rmw_create_wait_set 36 | : context, context->implementation_identifier, 37 | rmw_get_implementation_identifier(), return nullptr); 38 | 39 | rmw_wait_set_t * rmw_wait_set = nullptr; 40 | iox::popo::WaitSet * waitset = nullptr; 41 | 42 | rmw_wait_set = rmw_wait_set_allocate(); 43 | if (!rmw_wait_set) { 44 | RMW_SET_ERROR_MSG("failed to allocate wait set"); 45 | goto fail; 46 | } 47 | 48 | rmw_wait_set->implementation_identifier = rmw_get_implementation_identifier(); 49 | 50 | // create waitset 51 | waitset = static_cast *>( 52 | rmw_allocate(sizeof(iox::popo::WaitSet))); 53 | if (!waitset) { 54 | RMW_SET_ERROR_MSG("failed to allocate memory for wait_set data"); 55 | goto fail; 56 | } 57 | RMW_TRY_PLACEMENT_NEW( 58 | waitset, 59 | waitset, 60 | goto fail, 61 | iox::popo::WaitSet); 62 | 63 | rmw_wait_set->data = static_cast(waitset); 64 | return rmw_wait_set; 65 | 66 | fail: 67 | if (rmw_wait_set) { 68 | if (waitset) { 69 | RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( 70 | waitset->~WaitSet(), 71 | iox::popo::WaitSet) 72 | rmw_free(waitset); 73 | } 74 | 75 | rmw_wait_set_free(rmw_wait_set); 76 | } 77 | return nullptr; 78 | } 79 | 80 | rmw_ret_t 81 | rmw_destroy_wait_set(rmw_wait_set_t * wait_set) 82 | { 83 | RCUTILS_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_ERROR); 84 | 85 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 86 | rmw_destroy_wait_set 87 | : waitset, wait_set->implementation_identifier, 88 | rmw_get_implementation_identifier(), return RMW_RET_ERROR); 89 | 90 | rmw_ret_t result = RMW_RET_OK; 91 | 92 | auto iceoryx_wait_set = 93 | static_cast *>(wait_set->data); 94 | 95 | if (iceoryx_wait_set) { 96 | RMW_TRY_DESTRUCTOR( 97 | iceoryx_wait_set->~WaitSet(), 98 | iceoryx_wait_set, 99 | result = RMW_RET_ERROR) 100 | rmw_free(iceoryx_wait_set); 101 | } 102 | wait_set->data = nullptr; 103 | 104 | rmw_wait_set_free(wait_set); 105 | 106 | return result; 107 | } 108 | } // extern "C" 109 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/types/iceoryx_client.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022 - 2023 by Apex.AI Inc. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TYPES__ICEORYX_CLIENT_HPP_ 16 | #define TYPES__ICEORYX_CLIENT_HPP_ 17 | 18 | #include "../iceoryx_generate_gid.hpp" 19 | 20 | #include "iceoryx_posh/popo/untyped_client.hpp" 21 | 22 | #include "rmw/rmw.h" 23 | #include "rmw/types.h" 24 | 25 | #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" 26 | 27 | struct IceoryxClient 28 | { 29 | IceoryxClient( 30 | const rosidl_service_type_support_t * type_supports, 31 | iox::popo::UntypedClient * const iceoryx_client) 32 | : type_supports_(*type_supports), 33 | iceoryx_client_(iceoryx_client), 34 | is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)), 35 | request_size_(rmw_iceoryx_cpp::iceoryx_get_request_size(type_supports)), 36 | gid_(generate_client_gid(iceoryx_client_)) 37 | {} 38 | 39 | rosidl_service_type_support_t type_supports_; 40 | iox::popo::UntypedClient * const iceoryx_client_; 41 | bool is_fixed_size_{false}; 42 | size_t request_size_{0}; 43 | int64_t sequence_id_{0}; 44 | rmw_gid_t gid_; 45 | }; 46 | 47 | #endif // TYPES__ICEORYX_CLIENT_HPP_ 48 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/types/iceoryx_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TYPES__ICEORYX_NODE_HPP_ 16 | #define TYPES__ICEORYX_NODE_HPP_ 17 | 18 | #include 19 | 20 | #include "iceoryx_posh/popo/untyped_subscriber.hpp" 21 | #include "iceoryx_posh/roudi/introspection_types.hpp" 22 | #include "iceoryx_posh/runtime/node.hpp" 23 | #include "iceoryx_posh/popo/listener.hpp" 24 | 25 | #include "rcutils/error_handling.h" 26 | 27 | #include "rmw/impl/cpp/macros.hpp" 28 | #include "rmw/rmw.h" 29 | 30 | #include "../iceoryx_identifier.hpp" 31 | #include "iceoryx_posh/popo/user_trigger.hpp" 32 | 33 | // We currently use the iceoryx port introspection 34 | // which is updated whenever a sender port comes or goes or 35 | // does OFFER / STOP_OFFER or a receiver port comes or goes or does SUB / UNSUB 36 | /// @todo poehnl: check with the list of ros2 graph events 37 | 38 | class IceoryxGraphChangeNotifier 39 | { 40 | public: 41 | explicit IceoryxGraphChangeNotifier(rmw_guard_condition_t * guard_condition) 42 | { 43 | if (!guard_condition || !guard_condition->data) { 44 | RMW_SET_ERROR_MSG("invalid input for GraphChangeNotifier"); 45 | iceoryx_guard_condition_ = nullptr; 46 | } else { 47 | iceoryx_guard_condition_ = static_cast(guard_condition->data); 48 | RMW_CHECK_TYPE_IDENTIFIERS_MATCH( 49 | IceoryxGraphChangeNotifier 50 | : guard_condition, 51 | guard_condition->implementation_identifier, 52 | rmw_get_implementation_identifier(), 53 | iceoryx_guard_condition_ = nullptr); 54 | } 55 | 56 | /// @todo change to the dds_common graph 57 | // subscribe with a callback for changes in the iceoryx graph 58 | // https://github.com/eclipse-iceoryx/iceoryx/issues/707 59 | listener_.attachEvent( 60 | port_receiver_, iox::popo::SubscriberEvent::DATA_RECEIVED, 61 | iox::popo::createNotificationCallback(IceoryxGraphChangeNotifier::callback, *this)) 62 | .or_else( 63 | [](auto) { 64 | RMW_SET_ERROR_MSG("unable to attach port_receiver_"); 65 | std::terminate(); 66 | }); 67 | port_receiver_.subscribe(); 68 | } 69 | 70 | ~IceoryxGraphChangeNotifier() 71 | { 72 | listener_.detachEvent(port_receiver_, iox::popo::SubscriberEvent::DATA_RECEIVED); 73 | port_receiver_.unsubscribe(); 74 | } 75 | 76 | private: 77 | // must be a static method to be convertable to c function pointer 78 | // second argument (self) is the this pointer of the current object 79 | static void callback( 80 | iox::popo::UntypedSubscriber * introspectionSubscriber, 81 | IceoryxGraphChangeNotifier * self) 82 | { 83 | if (nullptr != self->iceoryx_guard_condition_) { 84 | self->iceoryx_guard_condition_->trigger(); 85 | } 86 | if (nullptr != introspectionSubscriber) { 87 | introspectionSubscriber->releaseQueuedData(); 88 | } 89 | } 90 | iox::popo::UserTrigger * iceoryx_guard_condition_{nullptr}; 91 | using port_receiver_t = iox::popo::UntypedSubscriber; 92 | port_receiver_t port_receiver_{iox::roudi::IntrospectionPortService, 93 | iox::popo::SubscriberOptions{1U, 1U, "", true}}; 94 | using listener_t = iox::popo::Listener; 95 | listener_t listener_; 96 | }; 97 | 98 | struct IceoryxNodeInfo 99 | { 100 | IceoryxNodeInfo( 101 | rmw_guard_condition_t * guard_condition, 102 | IceoryxGraphChangeNotifier * graph_change_notifier, 103 | iox::runtime::Node * iceoryx_runnable) 104 | : guard_condition_(guard_condition), 105 | graph_change_notifier_(graph_change_notifier), 106 | iceoryx_runnable_(iceoryx_runnable) 107 | { 108 | } 109 | rmw_guard_condition_t * const guard_condition_; 110 | IceoryxGraphChangeNotifier * const graph_change_notifier_; 111 | iox::runtime::Node * const iceoryx_runnable_; 112 | }; 113 | 114 | #endif // TYPES__ICEORYX_NODE_HPP_ 115 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/types/iceoryx_publisher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // Copyright (c) 2022 by Apex.AI Inc. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef TYPES__ICEORYX_PUBLISHER_HPP_ 17 | #define TYPES__ICEORYX_PUBLISHER_HPP_ 18 | 19 | #include "../iceoryx_generate_gid.hpp" 20 | 21 | #include "iceoryx_posh/popo/untyped_publisher.hpp" 22 | 23 | #include "rmw/rmw.h" 24 | #include "rmw/types.h" 25 | 26 | #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" 27 | 28 | struct IceoryxPublisher 29 | { 30 | IceoryxPublisher( 31 | const rosidl_message_type_support_t * type_supports, 32 | iox::popo::UntypedPublisher * const iceoryx_sender) 33 | : type_supports_(*type_supports), 34 | iceoryx_sender_(iceoryx_sender), 35 | gid_(generate_publisher_gid(iceoryx_sender_)), 36 | is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)), 37 | message_size_(rmw_iceoryx_cpp::iceoryx_get_message_size(type_supports)) 38 | {} 39 | 40 | rosidl_message_type_support_t type_supports_; 41 | iox::popo::UntypedPublisher * const iceoryx_sender_; 42 | rmw_gid_t gid_; 43 | bool is_fixed_size_; 44 | size_t message_size_; 45 | }; 46 | 47 | #endif // TYPES__ICEORYX_PUBLISHER_HPP_ 48 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/types/iceoryx_server.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022 - 2023 by Apex.AI Inc. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TYPES__ICEORYX_SERVER_HPP_ 16 | #define TYPES__ICEORYX_SERVER_HPP_ 17 | 18 | #include 19 | 20 | #include "iceoryx_posh/popo/untyped_server.hpp" 21 | 22 | #include "rmw/rmw.h" 23 | #include "rmw/types.h" 24 | 25 | #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" 26 | 27 | struct IceoryxServer 28 | { 29 | IceoryxServer( 30 | const rosidl_service_type_support_t * type_supports, 31 | iox::popo::UntypedServer * const iceoryx_server) 32 | : type_supports_(*type_supports), 33 | iceoryx_server_(iceoryx_server), 34 | is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)), 35 | response_size_(rmw_iceoryx_cpp::iceoryx_get_response_size(type_supports)) 36 | { 37 | } 38 | 39 | rosidl_service_type_support_t type_supports_; 40 | iox::popo::UntypedServer * const iceoryx_server_; 41 | bool is_fixed_size_{false}; 42 | size_t response_size_{0}; 43 | /// @brief The map stores the sequence numbers together with the corresponding 44 | /// sample pointer pointing to the shared memory. This is due to the fact that 45 | /// 'rmw_request_id_t' misses a place to store the sample pointer, which is not 46 | /// typical with DDS implementations. 47 | std::map request_payload_map_; 48 | }; 49 | 50 | #endif // TYPES__ICEORYX_SERVER_HPP_ 51 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/src/types/iceoryx_subscription.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TYPES__ICEORYX_SUBSCRIPTION_HPP_ 16 | #define TYPES__ICEORYX_SUBSCRIPTION_HPP_ 17 | 18 | #include "iceoryx_posh/popo/untyped_subscriber.hpp" 19 | 20 | #include "rmw/rmw.h" 21 | #include "rmw/types.h" 22 | 23 | #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" 24 | 25 | struct IceoryxSubscription 26 | { 27 | IceoryxSubscription( 28 | const rosidl_message_type_support_t * type_supports, 29 | iox::popo::UntypedSubscriber * const iceoryx_receiver) 30 | : type_supports_(*type_supports), 31 | iceoryx_receiver_(iceoryx_receiver), 32 | is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)), 33 | message_size_(rmw_iceoryx_cpp::iceoryx_get_message_size(type_supports)) 34 | {} 35 | 36 | rosidl_message_type_support_t type_supports_; 37 | iox::popo::UntypedSubscriber * const iceoryx_receiver_; 38 | bool is_fixed_size_; 39 | size_t message_size_; 40 | }; 41 | 42 | #endif // TYPES__ICEORYX_SUBSCRIPTION_HPP_ 43 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/test/iceoryx_fixed_size_messages_test.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" 16 | 17 | #include 18 | 19 | #include "rosidl_typesupport_cpp/message_type_support.hpp" 20 | 21 | #include "test_msgs/message_fixtures.hpp" 22 | 23 | #include "./test_msgs_c_fixtures.hpp" 24 | 25 | bool is_fixed_size(const rosidl_message_type_support_t * ts) 26 | { 27 | return rmw_iceoryx_cpp::iceoryx_is_fixed_size(ts); 28 | } 29 | 30 | template 31 | bool is_fixed_size() 32 | { 33 | auto ts = rosidl_typesupport_cpp::get_message_type_support_handle(); 34 | 35 | return is_fixed_size(ts); 36 | } 37 | 38 | TEST(FixedSizeMessagesTest, test_primitives) 39 | { 40 | EXPECT_TRUE(is_fixed_size()); 41 | EXPECT_TRUE(is_fixed_size()); 42 | EXPECT_TRUE(is_fixed_size()); 43 | EXPECT_TRUE(is_fixed_size()); 44 | EXPECT_TRUE(is_fixed_size()); 45 | } 46 | 47 | TEST(FixedSizeMessagesTest, test_complex_types) 48 | { 49 | EXPECT_FALSE(is_fixed_size()); 50 | EXPECT_FALSE(is_fixed_size()); 51 | EXPECT_FALSE(is_fixed_size()); 52 | EXPECT_FALSE(is_fixed_size()); 53 | EXPECT_FALSE(is_fixed_size()); 54 | EXPECT_FALSE(is_fixed_size()); 55 | } 56 | 57 | TEST(FixedSizeMessagesTest, test_cache_behavior) 58 | { 59 | EXPECT_FALSE(is_fixed_size()); 60 | 61 | EXPECT_FALSE(is_fixed_size()); 62 | EXPECT_FALSE(is_fixed_size()); 63 | EXPECT_FALSE(is_fixed_size()); 64 | EXPECT_FALSE(is_fixed_size()); 65 | EXPECT_FALSE(is_fixed_size()); 66 | } 67 | 68 | TEST(FixedSizeMessagesTest, test_primitives_c) 69 | { 70 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Empty); 71 | EXPECT_TRUE(is_fixed_size(ts)); 72 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); 73 | EXPECT_TRUE(is_fixed_size(ts)); 74 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Builtins); 75 | EXPECT_TRUE(is_fixed_size(ts)); 76 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Defaults); 77 | EXPECT_TRUE(is_fixed_size(ts)); 78 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Nested); 79 | EXPECT_TRUE(is_fixed_size(ts)); 80 | } 81 | 82 | TEST(FixedSizeMessagesTest, test_complex_types_c) 83 | { 84 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); 85 | EXPECT_FALSE(is_fixed_size(ts)); 86 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Arrays); 87 | EXPECT_FALSE(is_fixed_size(ts)); 88 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BoundedSequences); 89 | EXPECT_FALSE(is_fixed_size(ts)); 90 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, UnboundedSequences); 91 | EXPECT_FALSE(is_fixed_size(ts)); 92 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, MultiNested); 93 | EXPECT_FALSE(is_fixed_size(ts)); 94 | } 95 | 96 | TEST(FixedSizeMessagesTest, test_cache_behavior_c) 97 | { 98 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, MultiNested); 99 | EXPECT_FALSE(is_fixed_size(ts)); 100 | 101 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); 102 | EXPECT_FALSE(is_fixed_size(ts)); 103 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Arrays); 104 | EXPECT_FALSE(is_fixed_size(ts)); 105 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BoundedSequences); 106 | EXPECT_FALSE(is_fixed_size(ts)); 107 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, UnboundedSequences); 108 | EXPECT_FALSE(is_fixed_size(ts)); 109 | ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, MultiNested); 110 | EXPECT_FALSE(is_fixed_size(ts)); 111 | } 112 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/test/iceoryx_name_conversion_test.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | TEST(NameConverisonTests, get_name_n_type_from_service_description) 24 | { 25 | auto topic_and_type = rmw_iceoryx_cpp::get_name_n_type_from_service_description( 26 | "SERVICE", 27 | "INSTANCE", 28 | "EVENT"); 29 | EXPECT_EQ("/INSTANCE/SERVICE/EVENT", std::get<0>(topic_and_type)); 30 | EXPECT_EQ("service_ara_msgs/msg/EVENT", std::get<1>(topic_and_type)); 31 | } 32 | 33 | TEST(NameConverisonTests, get_service_description_from_name_n_type_revers) 34 | { 35 | auto service_description = rmw_iceoryx_cpp::get_service_description_from_name_n_type( 36 | "/INSTANCE/SERVICE/EVENT", 37 | "service_ara_msgs/msg/EVENT"); 38 | EXPECT_EQ("SERVICE", std::get<0>(service_description)); 39 | EXPECT_EQ("INSTANCE", std::get<1>(service_description)); 40 | EXPECT_EQ("EVENT", std::get<2>(service_description)); 41 | } 42 | 43 | TEST(NameConverisonTests, get_service_description_from_name_n_type) 44 | { 45 | auto service_description = rmw_iceoryx_cpp::get_service_description_from_name_n_type( 46 | "TopicName", 47 | "TypeName"); 48 | EXPECT_EQ("TypeName", std::get<0>(service_description)); 49 | EXPECT_EQ("TopicName", std::get<1>(service_description)); 50 | EXPECT_EQ("data", std::get<2>(service_description)); 51 | } 52 | 53 | TEST(NameConverisonTests, get_name_n_type_from_service_description_revers) 54 | { 55 | auto topic_and_type = rmw_iceoryx_cpp::get_name_n_type_from_service_description( 56 | "TypeName", 57 | "TopicName", 58 | "data"); 59 | EXPECT_EQ("TopicName", std::get<0>(topic_and_type)); 60 | EXPECT_EQ("TypeName", std::get<1>(topic_and_type)); 61 | } 62 | 63 | TEST(NameConverisonTests, get_hidden_introspection_topic) 64 | { 65 | auto topic_and_type = rmw_iceoryx_cpp::get_name_n_type_from_service_description( 66 | "Introspection", 67 | "RouDI_ID", 68 | "foo"); 69 | EXPECT_EQ(std::get<0>(topic_and_type), "/_iceoryx/RouDI_ID/Introspection/foo"); 70 | EXPECT_EQ(std::get<1>(topic_and_type), "iceoryx_introspection_msgs/msg/foo"); 71 | } 72 | 73 | TEST(NameConverisonTests, flip_flop) 74 | { 75 | std::vector> topic_names_and_types = 76 | {{"topic1", "type1"}, 77 | {"topic2", "type2"}, 78 | {"topic3", "type3"}, 79 | {"/_iceoryx/RouDI_1/Introspection/event1", "iceoryx_introspection_msgs/msg/event1"}, 80 | {"/_iceoryx/RouDI_1/Introspection/event2", "iceoryx_introspection_msgs/msg/event2"}}; 81 | 82 | for (auto & tuple : topic_names_and_types) { 83 | auto service_tuple = 84 | rmw_iceoryx_cpp::get_service_description_from_name_n_type( 85 | std::get<0>(tuple), 86 | std::get<1>(tuple)); 87 | auto flip_flop_tuple = rmw_iceoryx_cpp::get_name_n_type_from_service_description( 88 | std::get<0>(service_tuple), std::get<1>(service_tuple), std::get<2>(service_tuple)); 89 | EXPECT_EQ(tuple, flip_flop_tuple); 90 | } 91 | } 92 | 93 | TEST(NameConverisonTests, flip_flop_reverse) 94 | { 95 | std::vector> topic_names_and_types = 96 | {{"service1", "instance1", "event1"}, 97 | {"service2", "instance2", "event2"}, 98 | {"Introspection", "RouDI_1", "event1"}, 99 | {"type1", "topic1", "data"}}; 100 | 101 | for (auto & tuple : topic_names_and_types) { 102 | auto ros_tuple = rmw_iceoryx_cpp::get_name_n_type_from_service_description( 103 | std::get<0>(tuple), std::get<1>(tuple), std::get<2>(tuple)); 104 | auto flip_flop_tuple = 105 | rmw_iceoryx_cpp::get_service_description_from_name_n_type( 106 | std::get<0>(ros_tuple), 107 | std::get<1>(ros_tuple)); 108 | EXPECT_EQ(tuple, flip_flop_tuple); 109 | } 110 | } 111 | -------------------------------------------------------------------------------- /rmw_iceoryx_cpp/test/iceoryx_serialization_test.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" 16 | #include "rmw_iceoryx_cpp/iceoryx_serialize.hpp" 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "rosidl_typesupport_cpp/message_type_support.hpp" 25 | 26 | #include "rosidl_typesupport_introspection_cpp/identifier.hpp" 27 | #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" 28 | 29 | #include "test_msgs/message_fixtures.hpp" 30 | 31 | #include "./test_msgs_c_fixtures.hpp" 32 | 33 | template 34 | std::string to_string(const MessageT & msg) 35 | { 36 | (void) msg; 37 | return std::string("no to_string method defined") + typeid(MessageT).name(); 38 | } 39 | 40 | template<> 41 | std::string to_string(const test_msgs::msg::Strings & msg) 42 | { 43 | std::string str{}; 44 | str += "string_value: " + msg.string_value + "\n"; 45 | str += "bounded_string_value: " + msg.bounded_string_value + "\n"; 46 | return str; 47 | } 48 | 49 | template 50 | void test_equality(const MessageT & expected, const MessageT & actual) 51 | { 52 | EXPECT_EQ(expected, actual) << 53 | "expected: " << to_string(expected) << 54 | "got: " << to_string(actual); 55 | } 56 | 57 | template<> 58 | void test_equality(const test_msgs::msg::Strings & expected, const test_msgs::msg::Strings & actual) 59 | { 60 | EXPECT_STREQ(expected.string_value.c_str(), actual.string_value.c_str()); 61 | EXPECT_STREQ(expected.bounded_string_value.c_str(), actual.bounded_string_value.c_str()); 62 | } 63 | 64 | template< 65 | class MessageT, 66 | class MessageFixtureF = std::function>(void)> 67 | > 68 | void flip_flop_serialization(MessageFixtureF message_fixture) 69 | { 70 | auto ts = rosidl_typesupport_cpp::get_message_type_support_handle(); 71 | 72 | auto test_msgs = message_fixture(); 73 | for (auto i = 0u; i < test_msgs.size(); ++i) { 74 | fprintf(stderr, "+++ Message #%u +++\n", i); 75 | auto test_msg = test_msgs[i]; 76 | std::vector payload{}; 77 | 78 | MessageT * msg = test_msg.get(); 79 | rmw_iceoryx_cpp::serialize(msg, ts, payload); 80 | ASSERT_FALSE(payload.empty()); 81 | 82 | MessageT deserialized_msg{}; 83 | rmw_iceoryx_cpp::deserialize(payload.data(), ts, &deserialized_msg); 84 | 85 | test_equality(*msg, deserialized_msg); 86 | } 87 | } 88 | 89 | TEST(SerializationTests, cpp_flip_flop_serialize_empty) 90 | { 91 | flip_flop_serialization(std::bind(&get_messages_empty)); 92 | } 93 | 94 | TEST(SerializationTests, cpp_flip_flop_serialize_basic_types) 95 | { 96 | flip_flop_serialization(std::bind(&get_messages_basic_types)); 97 | } 98 | 99 | TEST(SerializationTests, cpp_flip_flop_serialize_constants) 100 | { 101 | flip_flop_serialization(std::bind(&get_messages_constants)); 102 | } 103 | 104 | TEST(SerializationTests, cpp_flip_flop_serialize_defaults) 105 | { 106 | flip_flop_serialization(std::bind(&get_messages_defaults)); 107 | } 108 | 109 | TEST(SerializationTests, cpp_flip_flop_serialize_Strings) 110 | { 111 | flip_flop_serialization(std::bind(&get_messages_strings)); 112 | } 113 | 114 | TEST(SerializationTests, cpp_flip_flop_serialize_arrays) 115 | { 116 | flip_flop_serialization(std::bind(&get_messages_arrays)); 117 | } 118 | 119 | TEST(SerializationTests, cpp_flip_flop_serialize_unbounded_sequences) 120 | { 121 | flip_flop_serialization( 122 | std::bind( 123 | & 124 | get_messages_unbounded_sequences)); 125 | } 126 | 127 | TEST(SerializationTests, cpp_flip_flop_serialize_bounded_sequences) 128 | { 129 | flip_flop_serialization( 130 | std::bind( 131 | & 132 | get_messages_bounded_sequences)); 133 | } 134 | 135 | TEST(SerializationTests, cpp_flip_flop_serialize_multi_nested) 136 | { 137 | flip_flop_serialization(std::bind(&get_messages_multi_nested)); 138 | } 139 | 140 | TEST(SerializationTests, cpp_flip_flop_serialize_nested) 141 | { 142 | flip_flop_serialization(std::bind(&get_messages_nested)); 143 | } 144 | 145 | TEST(SerializationTests, cpp_flip_flop_serialize_builtins) 146 | { 147 | flip_flop_serialization(std::bind(&get_messages_builtins)); 148 | } 149 | 150 | // TEST(SerializationTests, cpp_flip_flop_serialize_wstrings) 151 | // { 152 | // flip_flop_serialization(std::bind(&get_messages_wstrings)); 153 | // } 154 | 155 | template< 156 | class MessageT, 157 | class MessageFixtureF = std::function>(void)> 158 | > 159 | void flip_flop_serialization( 160 | MessageFixtureF message_fixture, 161 | const rosidl_message_type_support_t * ts) 162 | { 163 | auto test_msgs = message_fixture(); 164 | for (auto i = 0u; i < test_msgs.size(); ++i) { 165 | fprintf(stderr, "+++ Message #%u +++\n", i); 166 | auto test_msg = test_msgs[i]; 167 | std::vector payload{}; 168 | 169 | MessageT * msg = test_msg.get(); 170 | rmw_iceoryx_cpp::serialize(msg, ts, payload); 171 | ASSERT_FALSE(payload.empty()); 172 | 173 | MessageT deserialized_msg{}; 174 | rmw_iceoryx_cpp::deserialize(payload.data(), ts, &deserialized_msg); 175 | 176 | test_equality(*msg, deserialized_msg); 177 | } 178 | } 179 | 180 | TEST(SerializationTests, c_flip_flop_serialize_empty) 181 | { 182 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Empty); 183 | flip_flop_serialization(std::bind(&get_messages_empty_c), ts); 184 | } 185 | 186 | TEST(SerializationTests, c_flip_flop_serialize_basic_types) 187 | { 188 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); 189 | flip_flop_serialization(std::bind(&get_messages_basic_types_c), ts); 190 | } 191 | 192 | TEST(SerializationTests, c_flip_flop_serialize_constants) 193 | { 194 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Constants); 195 | flip_flop_serialization(std::bind(&get_messages_constants_c), ts); 196 | } 197 | 198 | TEST(SerializationTests, c_flip_flop_serialize_defaults) 199 | { 200 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Defaults); 201 | flip_flop_serialization(std::bind(&get_messages_defaults_c), ts); 202 | } 203 | 204 | TEST(SerializationTests, c_flip_flop_serialize_strings) 205 | { 206 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); 207 | flip_flop_serialization(std::bind(&get_messages_strings_c), ts); 208 | } 209 | 210 | TEST(SerializationTests, c_flip_flop_serialize_arrays) 211 | { 212 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Arrays); 213 | flip_flop_serialization(std::bind(&get_messages_arrays_c), ts); 214 | } 215 | 216 | TEST(SerializationTests, c_flip_flop_serialize_unbounded_sequences) 217 | { 218 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, UnboundedSequences); 219 | flip_flop_serialization( 220 | std::bind(&get_messages_unbounded_sequences_c), ts); 221 | } 222 | 223 | TEST(SerializationTests, c_flip_flop_serialize_bounded_sequences) 224 | { 225 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BoundedSequences); 226 | flip_flop_serialization( 227 | std::bind(&get_messages_bounded_sequences_c), ts); 228 | } 229 | 230 | TEST(SerializationTests, c_flip_flop_serialize_nested) 231 | { 232 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Nested); 233 | flip_flop_serialization(std::bind(&get_messages_nested_c), ts); 234 | } 235 | 236 | // TEST(SerializationTests, c_flip_flop_serialize_multinested) 237 | // { 238 | // auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, MultiNested); 239 | // flip_flop_serialization( 240 | // std::bind(&get_messages_multi_nested_c), ts); 241 | // } 242 | 243 | TEST(SerializationTests, c_flip_flop_serialize_builtins) 244 | { 245 | auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Builtins); 246 | flip_flop_serialization(std::bind(&get_messages_builtins_c), ts); 247 | } 248 | --------------------------------------------------------------------------------