From f1b80f0b37a6212d0b83552624db80232f3c56f7 Mon Sep 17 00:00:00 2001 From: VictorLee Date: Fri, 9 Jul 2021 03:37:16 +0800 Subject: [PATCH 1/9] Refactor the code with cpp api for iceoryx release Almond(1.0.1). (#42) * 1. Refactor code with cpp api, alter the interface for iceoryx 1.0.0. 2. Refactor iceoryx_ros2_bridge code left with a listener-adding work to do. 3. Update the workflow description file according to the work in master branch. 4. (done)Leaving waitset function to be done. 5. (done )Leaving listener function issue#707 to be solved by iceoryx team. 6. (commented the error msg)Remain waitset error msgs to debug. 7. (commented the error msg)Remain some other todo tags to be done. Signed-off-by: VictorLee * Rework changes from ZhenshengLee * Fix by-copy capture in lambda * Fix ChunkHeader and UserPayload mixup * Replace cxx::string usage Signed-off-by: Simon Hoinkis Co-authored-by: Johannes Jeising * Remove unnecessary casts Signed-off-by: Simon Hoinkis * Remove dummy receiver in rmw_wait_set and IceoryxWaitSet wrapper class Signed-off-by: Simon Hoinkis * Add SubscriberOptions to introspection subscriber Signed-off-by: Simon Hoinkis * Remove gcc warnings Signed-off-by: Simon Hoinkis * Replace IceoryxGuardCondition with popo::UserTrigger Signed-off-by: Simon Hoinkis * Fix rmw_wait Signed-off-by: Simon Hoinkis * Unify todo's and add copyright header Signed-off-by: Simon Hoinkis * Add limitation table and add some more todo's Signed-off-by: Simon Hoinkis * Run ament_uncrustify over all files Signed-off-by: Simon Hoinkis * Clarify iceoryx v2.0 features Signed-off-by: Simon Hoinkis * add support to rqt tools. Signed-off-by: VictorLee * add more feature states in readme doc Signed-off-by: VictorLee * Update copyright header. Co-authored-by: Simon Hoinkis Signed-off-by: VictorLee * fix lint and get ci work. Signed-off-by: VictorLee Co-authored-by: Simon Hoinkis Co-authored-by: Johannes Jeising --- .github/workflows/additional_repos.repos | 4 +- .github/workflows/lint.yml | 8 +- .github/workflows/main.yml | 13 +- .gitignore | 5 + README.md | 39 ++- .../src/iceoryx_ros2_bridge.cpp | 79 +++-- rmw_iceoryx_cpp/CMakeLists.txt | 1 + rmw_iceoryx_cpp/README.md | 11 + .../rmw_iceoryx_cpp/iceoryx_deserialize.hpp | 2 +- .../iceoryx_get_topic_endpoint_info.hpp | 86 ++++++ .../iceoryx_name_conversion.hpp | 10 + .../iceoryx_topic_names_and_types.hpp | 7 + rmw_iceoryx_cpp/package.xml | 2 +- .../src/internal/iceoryx_generate_gid.cpp | 2 +- .../iceoryx_get_topic_endpoint_info.cpp | 272 ++++++++++++++++++ .../src/internal/iceoryx_name_conversion.cpp | 19 +- .../iceoryx_topic_names_and_types.cpp | 98 +++++-- rmw_iceoryx_cpp/src/rmw_event.cpp | 4 +- .../src/rmw_get_topic_endpoint_info.cpp | 66 +++-- rmw_iceoryx_cpp/src/rmw_guard_condition.cpp | 21 +- rmw_iceoryx_cpp/src/rmw_init.cpp | 10 +- rmw_iceoryx_cpp/src/rmw_node.cpp | 24 +- .../src/rmw_node_info_and_types.cpp | 4 +- rmw_iceoryx_cpp/src/rmw_node_names.cpp | 49 ++-- rmw_iceoryx_cpp/src/rmw_publish.cpp | 50 ++-- rmw_iceoryx_cpp/src/rmw_publisher.cpp | 19 +- rmw_iceoryx_cpp/src/rmw_request.cpp | 2 + rmw_iceoryx_cpp/src/rmw_response.cpp | 3 + rmw_iceoryx_cpp/src/rmw_service.cpp | 3 +- rmw_iceoryx_cpp/src/rmw_subscription.cpp | 23 +- rmw_iceoryx_cpp/src/rmw_take.cpp | 104 ++++--- .../src/rmw_trigger_guard_condition.cpp | 5 +- rmw_iceoryx_cpp/src/rmw_wait.cpp | 110 +++---- rmw_iceoryx_cpp/src/rmw_wait_set.cpp | 68 ++--- .../src/types/iceoryx_guard_condition.hpp | 73 ----- rmw_iceoryx_cpp/src/types/iceoryx_node.hpp | 53 ++-- .../src/types/iceoryx_publisher.hpp | 6 +- .../src/types/iceoryx_subscription.hpp | 6 +- .../src/types/iceoryx_wait_set.hpp | 36 --- rmw_iceoryx_cpp/test/test_msgs_c_fixtures.hpp | 3 +- 40 files changed, 925 insertions(+), 475 deletions(-) create mode 100644 rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_get_topic_endpoint_info.hpp create mode 100644 rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp delete mode 100644 rmw_iceoryx_cpp/src/types/iceoryx_guard_condition.hpp delete mode 100644 rmw_iceoryx_cpp/src/types/iceoryx_wait_set.hpp diff --git a/.github/workflows/additional_repos.repos b/.github/workflows/additional_repos.repos index c36f5d2..c7c8519 100644 --- a/.github/workflows/additional_repos.repos +++ b/.github/workflows/additional_repos.repos @@ -1,5 +1,5 @@ repositories: eclipse/iceoryx: type: git - url: https://github.com/eclipse/iceoryx.git - version: master + url: https://github.com/eclipse-iceoryx/iceoryx.git + version: v1.0.1 diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index ba7c64e..2aa0fe2 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -5,15 +5,15 @@ on: jobs: lint: name: ament_${{ matrix.linter }} - runs-on: ubuntu-18.04 + runs-on: ubuntu-20.04 strategy: fail-fast: false matrix: linter: [cppcheck, cpplint, uncrustify, xmllint, copyright] steps: - - uses: actions/checkout@v1 - - uses: ros-tooling/setup-ros@0.0.23 - - uses: ros-tooling/action-ros-lint@0.0.6 + - uses: actions/checkout@v2 + - uses: ros-tooling/setup-ros@master + - uses: ros-tooling/action-ros-lint@master with: linter: ${{ matrix.linter }} package-name: | diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 2981fb2..4aeb0a1 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -12,16 +12,17 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-18.04] + os: [ubuntu-20.04] steps: + - uses: actions/checkout@v2 - name: Setup ROS - uses: ros-tooling/setup-ros@0.0.23 + uses: ros-tooling/setup-ros@master + with: + required-ros-distributions: foxy - name: Install Iceoryx Dependencies run: sudo apt-get update && sudo apt-get install -y cmake libacl1-dev libncurses5-dev pkg-config - name: Build & Test - uses: ros-tooling/action-ros-ci@0.0.17 + uses: ros-tooling/action-ros-ci@master with: package-name: rmw_iceoryx_cpp iceoryx_ros2_bridge - vcs-repo-file-url: | - https://gist.githubusercontent.com/Karsten1987/1723f219bb5aa5b81355c7eb1477f867/raw/9dc4d877f6fd0f862070516febdbddea28723b5c/iceoryx.repos - https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos + target-ros2-distro: foxy \ No newline at end of file diff --git a/.gitignore b/.gitignore index 1377554..84d938e 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,6 @@ *.swp +.vscode/ +build/ +install/ +log/ +rclcpp/ diff --git a/README.md b/README.md index 3138ce1..66c175e 100644 --- a/README.md +++ b/README.md @@ -7,10 +7,10 @@ Installation The following instructions show you how to install [iceoryx](https://github.com/eclipse/iceoryx) and its rmw implementation. The installation of iceoryx and rmw_iceoryx is pretty straight forward. -All provided packages can be built with colcon so that you can easily build both, iceoryx and rmw_iceoryx, within your ROS2 workspace. -rmw_iceoryx is using the [rosidl_typesupport_introspection](https://github.com/ros2/rosidl) which allows for building iceoryx on top of an existing ROS2 workspace or even debian installation as no ROS2 messages have to be built again. +All provided packages can be built with colcon so that you can easily build both, iceoryx and rmw_iceoryx, within your ROS 2 workspace. +rmw_iceoryx is using the [rosidl_typesupport_introspection](https://github.com/ros2/rosidl) which allows for building iceoryx on top of an existing ROS2 workspace or even debian installation as no ROS 2 messages have to be built again. -To install iceoryx and rmw_iceoryx in a ROS2 workspace, just execute the steps below: +To install iceoryx and rmw_iceoryx in a ROS 2 workspace, just execute the steps below: ``` mkdir -p ~/iceoryx_ws/src @@ -20,13 +20,15 @@ git clone https://github.com/ros2/rmw_iceoryx.git ``` For alternative installation instructions and more details about iceoryx's internals, please see [iceoryx's GitHub repo](https://github.com/eclipse/iceoryx). -rmw_iceoryx is compatible with ROS2 Eloquent. +rmw_iceoryx is compatible with ROS 2 Foxy. Assuming you have ROS2 installed correctly, you can compile the iceoryx workspace with colcon: ``` cd ~/iceoryx_ws/ -source /opt/ros/eloquent/setup.bash # alternatively source your own ROS2 workspace +source /opt/ros/foxy/setup.bash # alternatively source your own ROS 2 workspace colcon build +# or with more options +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF ``` That's it! You've installed iceoryx and are ready to rumble. @@ -82,7 +84,7 @@ rmw_iceoryx_cpp gives a loaned message to the subscription which can execute the Step 4 `return_loaned_message()`) A subscription callback is finished and the loaned message is getting returned to the middleware. -Starting from ROS2 Eloquent, these features are implemented within rclcpp. +Starting from ROS 2 Eloquent, these features are implemented within rclcpp. An application using these new features is shown in the code snippet below. 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). @@ -123,4 +125,27 @@ Limitations rmw_iceoryx_cpp is currently under heavy development. Unfortunately, not all features are yet fully fleshed out. -Other core functionalities like e.g. services are not yet implemented, but will follow soon. + +| ROS 2 command/feature | Status | +|-----------------------|------------------------------------| +| `ros2 topic list` | :heavy_check_mark: | +| `ros2 topic echo` | :heavy_check_mark: | +| `ros2 topic type` | :heavy_check_mark: | +| `ros2 topic info` | :heavy_check_mark: | +| `ros2 topic hz` | :heavy_check_mark: | +| `ros2 topic bw` | :heavy_check_mark: | +| `ros2 node list` | :heavy_check_mark: | +| `ros2 node info` | :heavy_check_mark: | +| `ros2 interface *` | :heavy_check_mark: | +| `ros2 service *` | :x: (coming with iceoryx v2.0) | +| `ros2 param list` | :x: (coming with iceoryx v2.0) | +| `rqt_graph` | :heavy_check_mark: | +| `rqt_top` | :heavy_check_mark: | +| `rqt_console` | :heavy_check_mark: | +| `rqt_plot` | :heavy_check_mark: | +| `rviz2` | :heavy_check_mark: | +| `ros2 bag` | :grey_question: | +| urdf | :grey_question: | +| tf2 | :grey_question: | +| ROS 2 bridge | :grey_question: | +| RMW Pub/Sub Events | :x: | diff --git a/iceoryx_ros2_bridge/src/iceoryx_ros2_bridge.cpp b/iceoryx_ros2_bridge/src/iceoryx_ros2_bridge.cpp index e0e0760..1784dff 100644 --- a/iceoryx_ros2_bridge/src/iceoryx_ros2_bridge.cpp +++ b/iceoryx_ros2_bridge/src/iceoryx_ros2_bridge.cpp @@ -16,8 +16,8 @@ #include #include -#include "iceoryx_posh/popo/publisher.hpp" -#include "iceoryx_posh/popo/subscriber.hpp" +#include "iceoryx_posh/popo/untyped_publisher.hpp" +#include "iceoryx_posh/popo/untyped_subscriber.hpp" #include "iceoryx_posh/runtime/posh_runtime.hpp" #include "rclcpp/rclcpp.hpp" @@ -61,21 +61,26 @@ bool deserialize_into( void publish_to_iceoryx( std::shared_ptr serialized_msg, const rosidl_message_type_support_t * ts, - std::shared_ptr iceoryx_publisher) + std::shared_ptr iceoryx_publisher) { auto introspection_ts = static_cast( get_message_typesupport_handle( ts, rosidl_typesupport_introspection_cpp::typesupport_identifier)->data); if (rmw_iceoryx_cpp::iceoryx_is_fixed_size(ts)) { - void * ros_msg = iceoryx_publisher->allocateChunk(introspection_ts->size_of_); - introspection_ts->init_function(ros_msg, rosidl_runtime_cpp::MessageInitialization::ALL); - - if (false == deserialize_into(serialized_msg.get(), ts, ros_msg)) { - iceoryx_publisher->freeChunk(ros_msg); - return; - } - iceoryx_publisher->sendChunk(ros_msg); + iceoryx_publisher->loan(introspection_ts->size_of_) + .and_then( + [&, introspection_ts](void * ros_msg) { + introspection_ts->init_function(ros_msg, rosidl_runtime_cpp::MessageInitialization::ALL); + if (false == deserialize_into(serialized_msg.get(), ts, ros_msg)) { + iceoryx_publisher->release(ros_msg); + } + iceoryx_publisher->publish(ros_msg); + }) + .or_else( + [](iox::popo::AllocationError) { + RMW_SET_ERROR_MSG("rmw_borrow_loaned_message error!"); + }); } else { void * ros_msg = malloc(introspection_ts->size_of_); introspection_ts->init_function(ros_msg, rosidl_runtime_cpp::MessageInitialization::ALL); @@ -91,9 +96,12 @@ void publish_to_iceoryx( payload_vector); free(ros_msg); - void * chunk = iceoryx_publisher->allocateChunk(payload_vector.size(), true); - memcpy(chunk, payload_vector.data(), payload_vector.size()); - iceoryx_publisher->sendChunk(chunk); + iceoryx_publisher->loan(payload_vector.size()) + .and_then( + [&, payload_vector](void * userPayload) { + memcpy(userPayload, payload_vector.data(), payload_vector.size()); + iceoryx_publisher->publish(userPayload); + }); } } @@ -114,19 +122,27 @@ bool serialize_into( } void publish_to_ros2( - std::shared_ptr subscriber, + std::shared_ptr subscriber, const rosidl_message_type_support_t * ts, std::shared_ptr ros2_publisher) { rclcpp::SerializedMessage serialized_msg; const void * chunk = nullptr; - subscriber->getChunk(&chunk); + subscriber->take() + .and_then( + [&](const void * userPayload) { + chunk = userPayload; + }) + .or_else( + [](iox::popo::ChunkReceiveResult) { + RMW_SET_ERROR_MSG("No chunk in subscriber"); + }); // ROS2 message is now in chunk. Convert to ROS2 message and then to serialized_message if (rmw_iceoryx_cpp::iceoryx_is_fixed_size(ts)) { if (false == serialize_into(chunk, ts, &serialized_msg)) { - subscriber->releaseChunk(chunk); + subscriber->release(chunk); return; } } else { @@ -144,7 +160,7 @@ void publish_to_ros2( ros_msg); if (false == serialize_into(ros_msg, ts, &serialized_msg)) { - subscriber->releaseChunk(chunk); + subscriber->release(chunk); return; } introspection_ts->fini_function(ros_msg); @@ -153,7 +169,7 @@ void publish_to_ros2( fprintf(stderr, "publishing message\n"); ros2_publisher->publish(&serialized_msg.get_rcl_serialized_message()); - subscriber->releaseChunk(chunk); + subscriber->release(chunk); } void usage() @@ -195,14 +211,17 @@ int main(int argc, char ** argv) start_parameter_event_publisher( false).enable_topic_statistics(false); auto node = std::make_shared(node_name, node_options); - iox::runtime::PoshRuntime::getInstance(std::string("/") + node_name); + iox::runtime::PoshRuntime::initRuntime( + iox::RuntimeName_t( + iox::cxx::TruncateToCapacity, + node_name)); /* * Subscribe to a ROS2 topic and re-publish into iceoryx */ std::vector> ros2_subs; ros2_subs.reserve(input_topics.size()); - std::vector> iceoryx_pubs; + std::vector> iceoryx_pubs; iceoryx_pubs.reserve(input_topics.size()); rclcpp::sleep_for(2s); // some room for ros2 discovery @@ -229,8 +248,9 @@ int main(int argc, char ** argv) auto service_desc = rmw_iceoryx_cpp::get_iceoryx_service_description(topic, ts); iceoryx_pubs.emplace_back( - std::make_shared( - service_desc, iox::cxx::CString100(iox::cxx::TruncateToCapacity, node_name))); + std::make_shared( + service_desc, iox::popo::PublisherOptions{ + 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_name)})); iceoryx_pubs.back()->offer(); std::function)> cb = @@ -245,7 +265,7 @@ int main(int argc, char ** argv) /* * Subscribe to an iceoryx topic and re-publish into ROS2 */ - std::vector> iceoryx_subs; + std::vector> iceoryx_subs; iceoryx_subs.reserve(output_topics.size()); std::vector> ros2_pubs; @@ -275,15 +295,18 @@ int main(int argc, char ** argv) auto service_desc = rmw_iceoryx_cpp::get_iceoryx_service_description(topic, ts); + /// @todo karsten1987: find a decent queue size instead of 10 iceoryx_subs.emplace_back( - std::make_shared( - service_desc, iox::cxx::CString100(iox::cxx::TruncateToCapacity, node_name))); - iceoryx_subs.back()->subscribe(10); // TODO(karsten1987): find a decent queue size + std::make_shared( + service_desc, iox::popo::SubscriberOptions{ + 10U, 0U, iox::cxx::string<100>(iox::cxx::TruncateToCapacity, node_name)})); + iceoryx_subs.back()->subscribe(); auto cb = std::bind(&publish_to_ros2, iceoryx_subs.back(), ts, ros2_pubs.back()); - iceoryx_subs.back()->setReceiveHandler(cb); + /// @todo add a listener + // iceoryx_subs.back()->setReceiveHandler(cb); } rclcpp::spin(node); diff --git a/rmw_iceoryx_cpp/CMakeLists.txt b/rmw_iceoryx_cpp/CMakeLists.txt index 692bc56..36eec3c 100644 --- a/rmw_iceoryx_cpp/CMakeLists.txt +++ b/rmw_iceoryx_cpp/CMakeLists.txt @@ -57,6 +57,7 @@ add_library(rmw_iceoryx_name_conversion SHARED src/internal/iceoryx_name_conversion.cpp src/internal/iceoryx_type_info_introspection.cpp src/internal/iceoryx_topic_names_and_types.cpp + src/internal/iceoryx_get_topic_endpoint_info.cpp ) target_include_directories(rmw_iceoryx_name_conversion PUBLIC include diff --git a/rmw_iceoryx_cpp/README.md b/rmw_iceoryx_cpp/README.md index 3a31ef3..b1e6bfe 100644 --- a/rmw_iceoryx_cpp/README.md +++ b/rmw_iceoryx_cpp/README.md @@ -2,3 +2,14 @@ RMW_ICEORYX_CPP --------------- C++ implementation of the rmw iceoryx middleware interface. + +## test + +```sh +/usr/local/bin/iox-roudi -l verbose + +export RMW_IMPLEMENTATION=rmw_iceoryx_cpp +ros2 run demo_nodes_cpp talker +ros2 run demo_nodes_cpp listener +iox-introspection-client --all +``` diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp index fd32457..ae82201 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_deserialize.hpp @@ -20,7 +20,7 @@ struct rosidl_message_type_support_t; namespace rmw_iceoryx_cpp { -// TODO(karsten1987): This should be `uint8`, really +/// @todo karsten1987: This should be `uint8`, really void deserialize( const char * serialized_msg, const rosidl_message_type_support_t * type_supports, diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_get_topic_endpoint_info.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_get_topic_endpoint_info.hpp new file mode 100644 index 0000000..11b3e93 --- /dev/null +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_get_topic_endpoint_info.hpp @@ -0,0 +1,86 @@ +// Copyright (c) 2021 by ZhenshengLee. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_ICEORYX_CPP__ICEORYX_GET_TOPIC_ENDPOINT_INFO_HPP_ +#define RMW_ICEORYX_CPP__ICEORYX_GET_TOPIC_ENDPOINT_INFO_HPP_ + +#include +#include +#include +#include + +#include "rcutils/types.h" + +#include "rmw/names_and_types.h" +#include "rmw/rmw.h" +#include "rmw/topic_endpoint_info_array.h" + +namespace rmw_iceoryx_cpp +{ +/** + * \brief Get topics and their publisher nodes + * \return std::map> + */ +std::map> get_publisher_and_nodes(); + +/** + * \brief Get topics and their subscriber nodes + * \return std::map> + */ +std::map> get_subscriber_and_nodes(); + +/** + * \brief Get publisher node names of one topic + * \param topic_name + * \return std::tuple> + */ +std::tuple> get_publisher_end_info_of_topic( + const char * topic_name); + +/** + * \brief Get subscriber node names of one topic + * \param topic_name + * \return std::tuple> + */ +std::tuple> get_subscriber_end_info_of_topic( + const char * topic_name); + +/** + * \brief helper function to fill rmw structs + * \param rmw_topic_endpoint_info_array + * \param iceoryx_topic_endpoint_info_array + * \param allocator + * \return rmw_ret_t + */ +rmw_ret_t +fill_rmw_publisher_end_info( + rmw_topic_endpoint_info_array_t * rmw_topic_endpoint_info_array, + const std::tuple> & iceoryx_topic_endpoint_info_array, + rcutils_allocator_t * allocator); + +/** + * \brief helper function to fill rmw structs + * \param rmw_topic_endpoint_info_array + * \param iceoryx_topic_endpoint_info_array + * \param allocator + * \return rmw_ret_t + */ +rmw_ret_t +fill_rmw_subscriber_end_info( + rmw_topic_endpoint_info_array_t * rmw_topic_endpoint_info_array, + const std::tuple> & iceoryx_topic_endpoint_info_array, + rcutils_allocator_t * allocator); + +} // namespace rmw_iceoryx_cpp +#endif // RMW_ICEORYX_CPP__ICEORYX_GET_TOPIC_ENDPOINT_INFO_HPP_ diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp index 47c50da..ddc8739 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_name_conversion.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by ZhenshengLee. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -25,6 +26,15 @@ struct rosidl_message_type_support_t; namespace rmw_iceoryx_cpp { +/// Get the pair of ros node name and namespace from a given iceoryx node full name. +/** + * \brief Get the name n space from node full name object + * \param node_full_name full name of the node + * \return std::tuple + */ +std::tuple +get_name_n_space_from_node_full_name( + const std::string & node_full_name); /// Get the pair of ROS topic and type from a given iceoryx service triplet. /** diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp index d938d8e..bc70e9b 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp @@ -27,6 +27,13 @@ namespace rmw_iceoryx_cpp { +void fill_topic_containers( + std::map & names_n_types_, + std::map> & subscribers_topics_, + std::map> & publishers_topics_, + std::map> & topic_subscribers_, + std::map> & topic_publishers_); + std::map get_topic_names_and_types(); std::map> get_nodes_and_publishers(); diff --git a/rmw_iceoryx_cpp/package.xml b/rmw_iceoryx_cpp/package.xml index 59beeef..e4f256e 100644 --- a/rmw_iceoryx_cpp/package.xml +++ b/rmw_iceoryx_cpp/package.xml @@ -2,7 +2,7 @@ rmw_iceoryx_cpp - 0.0.0 + 1.0.1 rmw implementation for Bosch's zero copy middleware iceoryx Karsten Knese Apache License 2.0 diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_generate_gid.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_generate_gid.cpp index c5d5898..5d400da 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_generate_gid.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_generate_gid.cpp @@ -18,7 +18,7 @@ rmw_gid_t generate_gid() { rmw_gid_t gid; - // TODO(mphnl) What would be the GUID for a sender in iceoryx? + /// @todo call iox::popo::BasePublisher::getUid() here gid.implementation_identifier = rmw_get_implementation_identifier(); memset(&gid.data[0], 0, RMW_GID_STORAGE_SIZE); diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp new file mode 100644 index 0000000..f59968a --- /dev/null +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_get_topic_endpoint_info.cpp @@ -0,0 +1,272 @@ +// Copyright (c) 2021 by ZhenshengLee. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "iceoryx_posh/popo/untyped_subscriber.hpp" +#include "iceoryx_posh/roudi/introspection_types.hpp" + +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" + +#include "rmw/impl/cpp/macros.hpp" + +#include "rmw_iceoryx_cpp/iceoryx_name_conversion.hpp" +#include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp" +#include "rmw_iceoryx_cpp/iceoryx_get_topic_endpoint_info.hpp" + +namespace rmw_iceoryx_cpp +{ +std::map> get_publisher_and_nodes() +{ + std::map names_n_types; + std::map> subscribers_topics; + std::map> publishers_topics; + std::map> topic_subscribers; + std::map> topic_publishers; + + fill_topic_containers( + names_n_types, subscribers_topics, publishers_topics, topic_subscribers, + topic_publishers); + + return topic_publishers; +} + +std::map> get_subscriber_and_nodes() +{ + std::map names_n_types; + std::map> subscribers_topics; + std::map> publishers_topics; + std::map> topic_subscribers; + std::map> topic_publishers; + + fill_topic_containers( + names_n_types, subscribers_topics, publishers_topics, topic_subscribers, + topic_publishers); + + return topic_subscribers; +} + +std::tuple> get_publisher_end_info_of_topic( + const char * topic_name) +{ + std::map names_n_types; + std::map> subscribers_topics; + std::map> publishers_topics; + std::map> topic_subscribers; + std::map> topic_publishers; + + fill_topic_containers( + names_n_types, subscribers_topics, publishers_topics, topic_subscribers, + topic_publishers); + auto full_name_array = topic_publishers[std::string(topic_name)]; + auto topic_type = names_n_types[topic_name]; + return std::make_tuple(topic_type, full_name_array); +} + +std::tuple> get_subscriber_end_info_of_topic( + const char * topic_name) +{ + std::map names_n_types; + std::map> subscribers_topics; + std::map> publishers_topics; + std::map> topic_subscribers; + std::map> topic_publishers; + + fill_topic_containers( + names_n_types, subscribers_topics, publishers_topics, topic_subscribers, + topic_publishers); + auto full_name_array = topic_subscribers[std::string(topic_name)]; + auto topic_type = names_n_types[topic_name]; + return std::make_tuple(topic_type, full_name_array); +} + +rmw_ret_t +fill_rmw_publisher_end_info( + rmw_topic_endpoint_info_array_t * rmw_topic_endpoint_info_array, + const std::tuple> & iceoryx_topic_endpoint_info, + rcutils_allocator_t * allocator) +{ + rmw_ret_t rmw_ret = RMW_RET_ERROR; + + auto topic_type = std::get<0>(iceoryx_topic_endpoint_info); + auto full_name_array = std::get<1>(iceoryx_topic_endpoint_info); + + if (!full_name_array.empty()) { + rmw_ret = rmw_topic_endpoint_info_array_init_with_size( + rmw_topic_endpoint_info_array, full_name_array.size(), + allocator); + if (rmw_ret != RMW_RET_OK) { + return rmw_ret; + } + } + + int i = 0; + // store all data in rmw_topic_endpoint_info_array_t + for (const auto node_full_name : full_name_array) { + auto name_n_space = get_name_n_space_from_node_full_name(node_full_name); + auto rmw_topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + // duplicate and store the topic_name + char * topic_type_cstr = rcutils_strdup(topic_type.c_str(), *allocator); + if (!topic_type_cstr) { + RMW_SET_ERROR_MSG("failed to allocate memory for topic_type"); + goto fail; + } + rmw_ret = rmw_topic_endpoint_info_set_topic_type( + &rmw_topic_endpoint_info, topic_type_cstr, + allocator); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + goto fail; + } + + char * node_name = rcutils_strdup(std::get<0>(name_n_space).c_str(), *allocator); + if (!node_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for node_name"); + goto fail; + } + rmw_ret = rmw_topic_endpoint_info_set_node_name(&rmw_topic_endpoint_info, node_name, allocator); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + goto fail; + } + + char * node_namespace = rcutils_strdup(std::get<1>(name_n_space).c_str(), *allocator); + if (!node_namespace) { + RMW_SET_ERROR_MSG("failed to allocate memory for node_namespace"); + goto fail; + } + rmw_ret = rmw_topic_endpoint_info_set_node_namespace( + &rmw_topic_endpoint_info, node_namespace, + allocator); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + goto fail; + } + + rmw_ret = rmw_topic_endpoint_info_set_endpoint_type( + &rmw_topic_endpoint_info, + RMW_ENDPOINT_PUBLISHER); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + goto fail; + } + /// @todo support gid and qos setting + + // set array + rmw_topic_endpoint_info_array->info_array[i] = rmw_topic_endpoint_info; + i++; + } + + return RMW_RET_OK; + +fail: + rmw_ret = rmw_topic_endpoint_info_array_fini(rmw_topic_endpoint_info_array, allocator); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + } + return RMW_RET_ERROR; +} + +rmw_ret_t +fill_rmw_subscriber_end_info( + rmw_topic_endpoint_info_array_t * rmw_topic_endpoint_info_array, + const std::tuple> & iceoryx_topic_endpoint_info, + rcutils_allocator_t * allocator) +{ + rmw_ret_t rmw_ret = RMW_RET_ERROR; + + auto topic_type = std::get<0>(iceoryx_topic_endpoint_info); + auto full_name_array = std::get<1>(iceoryx_topic_endpoint_info); + + if (!full_name_array.empty()) { + rmw_ret = rmw_topic_endpoint_info_array_init_with_size( + rmw_topic_endpoint_info_array, full_name_array.size(), + allocator); + if (rmw_ret != RMW_RET_OK) { + return rmw_ret; + } + } + + int i = 0; + // store all data in rmw_topic_endpoint_info_array_t + for (const auto node_full_name : full_name_array) { + auto name_n_space = get_name_n_space_from_node_full_name(node_full_name); + auto rmw_topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + // duplicate and store the topic_name + char * topic_type_cstr = rcutils_strdup(topic_type.c_str(), *allocator); + if (!topic_type_cstr) { + RMW_SET_ERROR_MSG("failed to allocate memory for topic_type"); + goto fail; + } + rmw_ret = rmw_topic_endpoint_info_set_topic_type( + &rmw_topic_endpoint_info, topic_type_cstr, + allocator); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + goto fail; + } + + char * node_name = rcutils_strdup(std::get<0>(name_n_space).c_str(), *allocator); + if (!node_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for node_name"); + goto fail; + } + rmw_ret = rmw_topic_endpoint_info_set_node_name(&rmw_topic_endpoint_info, node_name, allocator); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + goto fail; + } + + char * node_namespace = rcutils_strdup(std::get<1>(name_n_space).c_str(), *allocator); + if (!node_namespace) { + RMW_SET_ERROR_MSG("failed to allocate memory for node_namespace"); + goto fail; + } + rmw_ret = rmw_topic_endpoint_info_set_node_namespace( + &rmw_topic_endpoint_info, node_namespace, + allocator); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + goto fail; + } + + rmw_ret = rmw_topic_endpoint_info_set_endpoint_type( + &rmw_topic_endpoint_info, + RMW_ENDPOINT_SUBSCRIPTION); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + goto fail; + } + /// @todo support gid and qos setting + + // set array + rmw_topic_endpoint_info_array->info_array[i] = rmw_topic_endpoint_info; + i++; + } + + return RMW_RET_OK; + +fail: + rmw_ret = rmw_topic_endpoint_info_array_fini(rmw_topic_endpoint_info_array, allocator); + if (rmw_ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str); + } + return RMW_RET_ERROR; +} + +} // namespace rmw_iceoryx_cpp diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp index 371d6fe..89c0d05 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_name_conversion.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by ZhenshengLee. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -64,6 +65,18 @@ inline void extract_type( namespace rmw_iceoryx_cpp { +std::tuple +get_name_n_space_from_node_full_name( + const std::string & node_full_name) +{ + auto pos = node_full_name.rfind("/"); + + auto node_name = node_full_name.substr(pos + 1, node_full_name.size()); + auto node_namespace = node_full_name.substr(0, pos); + + return std::make_tuple(node_name, node_namespace); +} + std::tuple get_name_n_type_from_service_description( const std::string & service, @@ -159,9 +172,9 @@ get_iceoryx_service_description( auto serviceDescriptionTuple = get_service_description_from_name_n_type(topic_name, type_name); return iox::capro::ServiceDescription( - iox::capro::IdString(iox::cxx::TruncateToCapacity, std::get<0>(serviceDescriptionTuple)), - iox::capro::IdString(iox::cxx::TruncateToCapacity, std::get<1>(serviceDescriptionTuple)), - iox::capro::IdString(iox::cxx::TruncateToCapacity, std::get<2>(serviceDescriptionTuple))); + iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<0>(serviceDescriptionTuple)), + iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<1>(serviceDescriptionTuple)), + iox::capro::IdString_t(iox::cxx::TruncateToCapacity, std::get<2>(serviceDescriptionTuple))); } } // namespace rmw_iceoryx_cpp diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp index fb9ba9e..198bb08 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_topic_names_and_types.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,7 +17,7 @@ #include #include -#include "iceoryx_posh/popo/subscriber.hpp" +#include "iceoryx_posh/popo/untyped_subscriber.hpp" #include "iceoryx_posh/roudi/introspection_types.hpp" #include "rcutils/logging_macros.h" @@ -30,79 +31,98 @@ namespace rmw_iceoryx_cpp { -// TODO(mphnl) refactoring, too many copies +/// @todo Use the new service discovery API of iceoryx v2.0 here instead of introspection topics void fill_topic_containers( std::map & names_n_types_, std::map> & subscribers_topics_, - std::map> & publishers_topics_) + std::map> & publishers_topics_, + std::map> & topic_subscribers_, + std::map> & topic_publishers_) { - static iox::popo::Subscriber port_receiver(iox::roudi::IntrospectionPortService); + static iox::popo::UntypedSubscriber port_receiver(iox::roudi::IntrospectionPortService, + iox::popo::SubscriberOptions{1U, 1U, "", true}); static std::map names_n_types; static std::map> subscribers_topics; static std::map> publishers_topics; + static std::map> topic_subscribers; + static std::map> topic_publishers; bool updated = false; - if (iox::popo::SubscriptionState::SUBSCRIBED != port_receiver.getSubscriptionState()) { - port_receiver.subscribe(1); + if (iox::SubscribeState::SUBSCRIBED != port_receiver.getSubscriptionState()) { + port_receiver.subscribe(); // wait for delivery on subscribe - while (!port_receiver.hasNewChunks()) { + while (!port_receiver.hasData()) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); } updated = true; } else { - updated = port_receiver.hasNewChunks(); + updated = port_receiver.hasData(); } if (updated) { // get the latest sample - const iox::mepoo::ChunkHeader * chunk_header = nullptr; - const iox::mepoo::ChunkHeader * latest_chunk_header = nullptr; - - while (port_receiver.getChunk(&chunk_header)) { - if (latest_chunk_header) { - port_receiver.releaseChunk(latest_chunk_header); - } - latest_chunk_header = chunk_header; + const void * previous_user_payload = nullptr; + + while (port_receiver.take() + .and_then( + [&](const void * userPayload) { + if (previous_user_payload) { + port_receiver.release(previous_user_payload); + } + previous_user_payload = userPayload; + }) + .or_else( + [](auto & result) { + if (result != iox::popo::ChunkReceiveResult::NO_CHUNK_AVAILABLE) { + RMW_SET_ERROR_MSG("failed to take message"); + } + })) + + { } - if (latest_chunk_header) { + if (previous_user_payload) { const iox::roudi::PortIntrospectionFieldTopic * port_sample = - static_cast(latest_chunk_header-> - payload()); + static_cast(previous_user_payload); names_n_types.clear(); subscribers_topics.clear(); publishers_topics.clear(); - for (auto & receiver : port_sample->m_receiverList) { + for (auto & receiver : port_sample->m_subscriberList) { auto name_and_type = rmw_iceoryx_cpp::get_name_n_type_from_service_description( std::string(receiver.m_caproServiceID.c_str()), std::string(receiver.m_caproInstanceID.c_str()), std::string(receiver.m_caproEventMethodID.c_str())); names_n_types[std::get<0>(name_and_type)] = std::get<1>(name_and_type); - subscribers_topics[std::string(receiver.m_runnable.c_str())].push_back( + subscribers_topics[std::string(receiver.m_node.c_str())].push_back( std::get<0>( name_and_type)); + topic_subscribers[std::get<0>(name_and_type)].push_back( + std::string(receiver.m_node.c_str())); } - for (auto & sender : port_sample->m_senderList) { + for (auto & sender : port_sample->m_publisherList) { auto name_and_type = rmw_iceoryx_cpp::get_name_n_type_from_service_description( std::string(sender.m_caproServiceID.c_str()), std::string(sender.m_caproInstanceID.c_str()), std::string(sender.m_caproEventMethodID.c_str())); names_n_types[std::get<0>(name_and_type)] = std::get<1>(name_and_type); - publishers_topics[std::string(sender.m_runnable.c_str())].push_back( + publishers_topics[std::string(sender.m_node.c_str())].push_back( std::get<0>( name_and_type)); + topic_publishers[std::get<0>(name_and_type)].push_back(std::string(sender.m_node.c_str())); } - port_receiver.releaseChunk(latest_chunk_header); + port_receiver.release(previous_user_payload); } } names_n_types_ = names_n_types; subscribers_topics_ = subscribers_topics; publishers_topics_ = publishers_topics; + topic_subscribers_ = topic_subscribers; + topic_publishers_ = topic_publishers; } std::map get_topic_names_and_types() @@ -110,8 +130,12 @@ std::map get_topic_names_and_types() std::map names_n_types; std::map> subscribers_topics; std::map> publishers_topics; + std::map> topic_subscribers; + std::map> topic_publishers; - fill_topic_containers(names_n_types, subscribers_topics, publishers_topics); + fill_topic_containers( + names_n_types, subscribers_topics, publishers_topics, topic_subscribers, + topic_publishers); return names_n_types; } @@ -121,8 +145,12 @@ std::map> get_nodes_and_publishers() std::map names_n_types; std::map> subscribers_topics; std::map> publishers_topics; + std::map> topic_subscribers; + std::map> topic_publishers; - fill_topic_containers(names_n_types, subscribers_topics, publishers_topics); + fill_topic_containers( + names_n_types, subscribers_topics, publishers_topics, topic_subscribers, + topic_publishers); return publishers_topics; } @@ -132,8 +160,12 @@ std::map> get_nodes_and_subscribers() std::map names_n_types; std::map> subscribers_topics; std::map> publishers_topics; + std::map> topic_subscribers; + std::map> topic_publishers; - fill_topic_containers(names_n_types, subscribers_topics, publishers_topics); + fill_topic_containers( + names_n_types, subscribers_topics, publishers_topics, topic_subscribers, + topic_publishers); return subscribers_topics; } @@ -145,8 +177,12 @@ std::map get_publisher_names_and_types_of_node( std::map names_n_types; std::map> subscribers_topics; std::map> publishers_topics; + std::map> topic_subscribers; + std::map> topic_publishers; - fill_topic_containers(names_n_types, subscribers_topics, publishers_topics); + fill_topic_containers( + names_n_types, subscribers_topics, publishers_topics, topic_subscribers, + topic_publishers); std::map publisher_names_and_types; @@ -166,8 +202,12 @@ std::map get_subscription_names_and_types_of_node( std::map names_n_types; std::map> subscribers_topics; std::map> publishers_topics; + std::map> topic_subscribers; + std::map> topic_publishers; - fill_topic_containers(names_n_types, subscribers_topics, publishers_topics); + fill_topic_containers( + names_n_types, subscribers_topics, publishers_topics, topic_subscribers, + topic_publishers); std::map subscriber_names_and_types; diff --git a/rmw_iceoryx_cpp/src/rmw_event.cpp b/rmw_iceoryx_cpp/src/rmw_event.cpp index bed8ef2..75f44e0 100644 --- a/rmw_iceoryx_cpp/src/rmw_event.cpp +++ b/rmw_iceoryx_cpp/src/rmw_event.cpp @@ -29,7 +29,7 @@ rmw_publisher_event_init( RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); (void) event_type; - RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support publisher events."); + /// @todo add publisher events support return RMW_RET_UNSUPPORTED; } @@ -43,7 +43,7 @@ rmw_subscription_event_init( RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); (void) event_type; - RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support subscription events."); + /// @todo add subscription events support return RMW_RET_UNSUPPORTED; } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_iceoryx_cpp/src/rmw_get_topic_endpoint_info.cpp index 7bcdde0..acc3064 100644 --- a/rmw_iceoryx_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_iceoryx_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -18,41 +18,63 @@ #include "rmw/rmw.h" #include "rmw/topic_endpoint_info_array.h" -extern "C" -{ -rmw_ret_t -rmw_get_publishers_info_by_topic( - const rmw_node_t * node, - rcutils_allocator_t * allocator, - const char * topic_name, - bool no_mangle, +#include "rmw_iceoryx_cpp/iceoryx_topic_names_and_types.hpp" +#include "rmw_iceoryx_cpp/iceoryx_get_topic_endpoint_info.hpp" + +extern "C" { +rmw_ret_t rmw_get_publishers_info_by_topic( + const rmw_node_t * node, rcutils_allocator_t * allocator, + const char * topic_name, bool no_mangle, rmw_topic_endpoint_info_array_t * publishers_info) { RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_ERROR); - (void) no_mangle; - (void) publishers_info; + (void)no_mangle; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + rmw_get_topic_names_and_types + : node, node->implementation_identifier, rmw_get_implementation_identifier(), + return RMW_RET_ERROR); + + rmw_ret_t rmw_ret = rmw_topic_endpoint_info_array_check_zero(publishers_info); + if (rmw_ret != RMW_RET_OK) { + return rmw_ret; // error already set + } - RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support publishers info."); - return RMW_RET_UNSUPPORTED; + auto iceoryx_publisher_endpoint_info = + rmw_iceoryx_cpp::get_publisher_end_info_of_topic(topic_name); + + return rmw_iceoryx_cpp::fill_rmw_publisher_end_info( + publishers_info, + iceoryx_publisher_endpoint_info, allocator); } -rmw_ret_t -rmw_get_subscriptions_info_by_topic( - const rmw_node_t * node, - rcutils_allocator_t * allocator, - const char * topic_name, - bool no_mangle, +rmw_ret_t rmw_get_subscriptions_info_by_topic( + const rmw_node_t * node, rcutils_allocator_t * allocator, + const char * topic_name, bool no_mangle, rmw_topic_endpoint_info_array_t * subscriptions_info) { RCUTILS_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_ERROR); - (void) no_mangle; - (void) subscriptions_info; + (void)no_mangle; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + rmw_get_topic_names_and_types + : node, node->implementation_identifier, rmw_get_implementation_identifier(), + return RMW_RET_ERROR); + + rmw_ret_t rmw_ret = rmw_topic_endpoint_info_array_check_zero(subscriptions_info); + if (rmw_ret != RMW_RET_OK) { + return rmw_ret; // error already set + } + + auto iceoryx_subscriber_endpoint_info = rmw_iceoryx_cpp::get_subscriber_end_info_of_topic( + topic_name); - RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support subscriptions info."); - return RMW_RET_UNSUPPORTED; + return rmw_iceoryx_cpp::fill_rmw_subscriber_end_info( + subscriptions_info, iceoryx_subscriber_endpoint_info, + allocator); } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_guard_condition.cpp b/rmw_iceoryx_cpp/src/rmw_guard_condition.cpp index 7fce690..c7e9355 100644 --- a/rmw_iceoryx_cpp/src/rmw_guard_condition.cpp +++ b/rmw_iceoryx_cpp/src/rmw_guard_condition.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,7 +21,7 @@ #include "rmw/rmw.h" #include "./iceoryx_identifier.hpp" -#include "./types/iceoryx_guard_condition.hpp" +#include "iceoryx_posh/popo/user_trigger.hpp" extern "C" { @@ -34,12 +35,12 @@ rmw_create_guard_condition(rmw_context_t * context) : context, context->implementation_identifier, rmw_get_implementation_identifier(), - // TODO(wwjwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when - // possible + /// @todo wwjwood: replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when + /// possible return nullptr); rmw_guard_condition_t * guard_condition = nullptr; - IceoryxGuardCondition * iceoryx_guard_condition = nullptr; + iox::popo::UserTrigger * iceoryx_guard_condition = nullptr; guard_condition = rmw_guard_condition_allocate(); if (!guard_condition) { @@ -48,8 +49,8 @@ rmw_create_guard_condition(rmw_context_t * context) } guard_condition->implementation_identifier = rmw_get_implementation_identifier(); - iceoryx_guard_condition = static_cast( - rmw_allocate(sizeof(IceoryxGuardCondition))); + iceoryx_guard_condition = static_cast( + rmw_allocate(sizeof(iox::popo::UserTrigger))); if (!iceoryx_guard_condition) { RMW_SET_ERROR_MSG("failed to construct guard condition data"); goto fail; @@ -59,7 +60,7 @@ rmw_create_guard_condition(rmw_context_t * context) iceoryx_guard_condition, goto fail, // cppcheck-suppress syntaxError - IceoryxGuardCondition, ) + iox::popo::UserTrigger, ) guard_condition->data = iceoryx_guard_condition; return guard_condition; @@ -68,7 +69,7 @@ rmw_create_guard_condition(rmw_context_t * context) if (guard_condition) { if (iceoryx_guard_condition) { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - iceoryx_guard_condition->~IceoryxGuardCondition(), iceoryx_guard_condition) + iceoryx_guard_condition->~UserTrigger(), iceoryx_guard_condition) rmw_free(iceoryx_guard_condition); } rmw_guard_condition_free(guard_condition); @@ -89,12 +90,12 @@ rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) rmw_get_implementation_identifier(), return RMW_RET_ERROR); - auto iceoryx_guard_condition = static_cast(guard_condition->data); + auto iceoryx_guard_condition = static_cast(guard_condition->data); auto result = RMW_RET_OK; if (iceoryx_guard_condition) { RMW_TRY_DESTRUCTOR( - iceoryx_guard_condition->~IceoryxGuardCondition(), + iceoryx_guard_condition->~UserTrigger(), iceoryx_guard_condition, result = RMW_RET_ERROR) rmw_free(iceoryx_guard_condition); diff --git a/rmw_iceoryx_cpp/src/rmw_init.cpp b/rmw_iceoryx_cpp/src/rmw_init.cpp index 9649161..7704085 100644 --- a/rmw_iceoryx_cpp/src/rmw_init.cpp +++ b/rmw_iceoryx_cpp/src/rmw_init.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -96,15 +97,14 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // create a name for the process to register with the RouDi daemon extern char * __progname; auto progName = std::string(__progname); - auto name = std::string("/") + progName + "_" + std::to_string(getpid()); - - // TODO(mphnl) Would it make sense to check if thr RouDi daemon is running - // and to start it here if not? + /// @todo we could check with the introspection topics beforehand if the name is already used + auto name = progName + "_" + std::to_string(getpid()); // This call creates a runtime object. // It regisers with the RouDi daemon and gets the configuration // for setting up the shared memeory - iox::runtime::PoshRuntime::getInstance(name); + iox::log::LogManager::GetLogManager().SetDefaultLogLevel(iox::log::LogLevel::kWarn); + iox::runtime::PoshRuntime::initRuntime(iox::RuntimeName_t(iox::cxx::TruncateToCapacity, name)); return RMW_RET_OK; } diff --git a/rmw_iceoryx_cpp/src/rmw_node.cpp b/rmw_iceoryx_cpp/src/rmw_node.cpp index 7b8b1b1..ae9c019 100644 --- a/rmw_iceoryx_cpp/src/rmw_node.cpp +++ b/rmw_iceoryx_cpp/src/rmw_node.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,7 +15,7 @@ #include -#include "iceoryx_posh/runtime/runnable.hpp" +#include "iceoryx_posh/runtime/node.hpp" #include "rmw/allocators.h" @@ -40,7 +41,7 @@ rmw_create_node( std::string full_name = std::string(namespace_) + std::string(name); rmw_guard_condition_t * guard_condition = nullptr; IceoryxGraphChangeNotifier * graph_change_notifier = nullptr; - iox::runtime::Runnable * iceoryx_runnable = nullptr; + iox::runtime::Node * iceoryx_runnable = nullptr; IceoryxNodeInfo * node_info = nullptr; rmw_node_t * node_handle = rmw_node_allocate(); @@ -57,8 +58,7 @@ rmw_create_node( goto fail; } - // TODO(knese) what is the scope of the guard condition, for the node only or the whole system? - // If the all changes in the system are relevant, one GraphChangeNotifier for all nodes would do? + /// @todo only use one GraphChangeNotifier for all nodes graph_change_notifier = static_cast(rmw_allocate(sizeof(IceoryxGraphChangeNotifier))); if (!graph_change_notifier) { @@ -71,16 +71,16 @@ rmw_create_node( // allocate iceoryx_runnable iceoryx_runnable = - static_cast(rmw_allocate( - sizeof(iox::runtime::Runnable))); + static_cast(rmw_allocate( + sizeof(iox::runtime::Node))); if (!iceoryx_runnable) { RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx runnable"); goto fail; } RMW_TRY_PLACEMENT_NEW( iceoryx_runnable, iceoryx_runnable, - goto fail, iox::runtime::Runnable, - iox::cxx::CString100(iox::cxx::TruncateToCapacity, full_name)); + goto fail, iox::runtime::Node, + iox::NodeName_t(iox::cxx::TruncateToCapacity, full_name)); node_info = static_cast(rmw_allocate(sizeof(IceoryxNodeInfo))); if (!node_info) { @@ -124,7 +124,7 @@ rmw_create_node( } if (iceoryx_runnable) { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - iceoryx_runnable->~Runnable(), iox::runtime::Runnable) + iceoryx_runnable->~Node(), iox::runtime::Node) rmw_free(iceoryx_runnable); } if (node_info) { @@ -168,7 +168,7 @@ rmw_destroy_node(rmw_node_t * node) } if (node_info->iceoryx_runnable_) { RMW_TRY_DESTRUCTOR( - node_info->iceoryx_runnable_->~Runnable(), + node_info->iceoryx_runnable_->~Node(), node_info->iceoryx_runnable_, result = RMW_RET_ERROR) rmw_free(node_info->iceoryx_runnable_); @@ -202,8 +202,8 @@ rmw_node_assert_liveliness(const rmw_node_t * node) : node, node->implementation_identifier, rmw_get_implementation_identifier(), return RMW_RET_ERROR); - // TODO(mphnl): Currently the heartbeat is used that every registered process in iceoryx sends - // later this should be extended to a user triggered liveliness that can be send for a runnable + /// @todo poehnl: Currently the heartbeat is used that every registered process in iceoryx sends + /// later this should be extended to a user triggered liveliness that can be send for a runnable return RMW_RET_OK; } diff --git a/rmw_iceoryx_cpp/src/rmw_node_info_and_types.cpp b/rmw_iceoryx_cpp/src/rmw_node_info_and_types.cpp index 1256087..a8bcea8 100644 --- a/rmw_iceoryx_cpp/src/rmw_node_info_and_types.cpp +++ b/rmw_iceoryx_cpp/src/rmw_node_info_and_types.cpp @@ -105,7 +105,7 @@ rmw_get_service_names_and_types_by_node( RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_namespace, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_ERROR); - // TODO(mphnl) implementation + /// @todo poehnl: implementation return RMW_RET_OK; } @@ -124,7 +124,7 @@ rmw_get_client_names_and_types_by_node( RCUTILS_CHECK_ARGUMENT_FOR_NULL(node_namespace, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_ERROR); - // TODO(mphnl) implementation + /// @todo poehnl: implementation return RMW_RET_OK; } diff --git a/rmw_iceoryx_cpp/src/rmw_node_names.cpp b/rmw_iceoryx_cpp/src/rmw_node_names.cpp index 0c7bfef..262ea22 100644 --- a/rmw_iceoryx_cpp/src/rmw_node_names.cpp +++ b/rmw_iceoryx_cpp/src/rmw_node_names.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,7 +16,7 @@ #include #include -#include "iceoryx_posh/popo/subscriber.hpp" +#include "iceoryx_posh/popo/untyped_subscriber.hpp" #include "iceoryx_posh/roudi/introspection_types.hpp" #include "rcutils/error_handling.h" @@ -43,45 +44,53 @@ rmw_get_node_names( : node, node->implementation_identifier, rmw_get_implementation_identifier(), return RMW_RET_ERROR); - static iox::popo::Subscriber process_receiver(iox::roudi::IntrospectionProcessService); + static iox::popo::UntypedSubscriber process_receiver(iox::roudi::IntrospectionProcessService, + iox::popo::SubscriberOptions{1U, 1U, "", true}); static std::set node_names_set; bool updated = false; - if (iox::popo::SubscriptionState::SUBSCRIBED != process_receiver.getSubscriptionState()) { - process_receiver.subscribe(1); + if (iox::SubscribeState::SUBSCRIBED != process_receiver.getSubscriptionState()) { + process_receiver.subscribe(); // wait for delivery on subscribe - while (!process_receiver.hasNewChunks()) { + while (!process_receiver.hasData()) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); } updated = true; } else { - updated = process_receiver.hasNewChunks(); + updated = process_receiver.hasData(); } if (updated) { - // get the latest sample - const iox::mepoo::ChunkHeader * chunk_header = nullptr; - const iox::mepoo::ChunkHeader * latest_chunk_header = nullptr; - - while (process_receiver.getChunk(&chunk_header)) { - if (latest_chunk_header) { - process_receiver.releaseChunk(latest_chunk_header); - } - latest_chunk_header = chunk_header; + const void * previous_user_payload = nullptr; + + while (process_receiver.take() + .and_then( + [&](const void * userPayload) { + if (previous_user_payload) { + process_receiver.release(previous_user_payload); + } + previous_user_payload = userPayload; + }) + .or_else( + [](auto & result) { + if (result != iox::popo::ChunkReceiveResult::NO_CHUNK_AVAILABLE) { + RMW_SET_ERROR_MSG("failed to take message"); + } + })) + { } - if (latest_chunk_header) { + if (previous_user_payload) { const iox::roudi::ProcessIntrospectionFieldTopic * process_sample = - static_cast(latest_chunk_header-> - payload()); + static_cast(previous_user_payload); node_names_set.clear(); for (auto & process : process_sample->m_processList) { - for (auto & runnable : process.m_runnables) { + for (auto & runnable : process.m_nodes) { node_names_set.insert(std::string(runnable.c_str())); } } - process_receiver.releaseChunk(latest_chunk_header); + process_receiver.release(previous_user_payload); } } diff --git a/rmw_iceoryx_cpp/src/rmw_publish.cpp b/rmw_iceoryx_cpp/src/rmw_publish.cpp index 0869168..869b935 100644 --- a/rmw_iceoryx_cpp/src/rmw_publish.cpp +++ b/rmw_iceoryx_cpp/src/rmw_publish.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,8 +15,7 @@ #include -#include "iceoryx_posh/mepoo/chunk_info.hpp" -#include "iceoryx_posh/popo/publisher.hpp" +#include "iceoryx_posh/popo/untyped_publisher.hpp" #include "rcutils/error_handling.h" @@ -39,7 +39,7 @@ namespace details { rmw_ret_t send_payload( - iox::popo::Publisher * iceoryx_publisher, + iox::popo::UntypedPublisher * iceoryx_publisher, const void * serialized_ros_msg, size_t size) { @@ -47,13 +47,20 @@ send_payload( RMW_SET_ERROR_MSG("serialized message pointer is null"); return RMW_RET_ERROR; } - void * chunk = iceoryx_publisher->allocateChunk(size, true); - - memcpy(chunk, serialized_ros_msg, size); - - iceoryx_publisher->sendChunk(chunk); - - return RMW_RET_OK; + rmw_ret_t ret = RMW_RET_ERROR; + iceoryx_publisher->loan(size) + .and_then( + [&](void * userPayload) { + memcpy(userPayload, serialized_ros_msg, size); + iceoryx_publisher->publish(userPayload); + ret = RMW_RET_OK; + }) + .or_else( + [&](iox::popo::AllocationError) { + RMW_SET_ERROR_MSG("send_payload error!"); + ret = RMW_RET_ERROR; + }); + return ret; } } // namespace details @@ -168,11 +175,20 @@ rmw_borrow_loaned_message( return RMW_RET_ERROR; } - auto msg_memory = iceoryx_sender->allocateChunk( - static_cast(iceoryx_publisher->message_size_), true); - rmw_iceoryx_cpp::iceoryx_init_message(&iceoryx_publisher->type_supports_, msg_memory); - *ros_message = msg_memory; - return RMW_RET_OK; + rmw_ret_t ret = RMW_RET_ERROR; + iceoryx_sender->loan(iceoryx_publisher->message_size_) + .and_then( + [&](void * msg_memory) { + rmw_iceoryx_cpp::iceoryx_init_message(&iceoryx_publisher->type_supports_, msg_memory); + *ros_message = msg_memory; + ret = RMW_RET_OK; + }) + .or_else( + [&](iox::popo::AllocationError) { + RMW_SET_ERROR_MSG("rmw_borrow_loaned_message error!"); + ret = RMW_RET_ERROR; + }); + return ret; } rmw_ret_t @@ -205,7 +221,7 @@ rmw_return_loaned_message_from_publisher(const rmw_publisher_t * publisher, void } rmw_iceoryx_cpp::iceoryx_fini_message(&iceoryx_publisher->type_supports_, loaned_message); - iceoryx_sender->freeChunk(loaned_message); + iceoryx_sender->release(loaned_message); return RMW_RET_OK; } @@ -241,7 +257,7 @@ rmw_publish_loaned_message( RMW_SET_ERROR_MSG("iceoryx can't loan non-fixed sized messages"); return RMW_RET_ERROR; } - iceoryx_sender->sendChunk(ros_message); + iceoryx_sender->publish(ros_message); return RMW_RET_OK; } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_publisher.cpp b/rmw_iceoryx_cpp/src/rmw_publisher.cpp index 69bc4ca..f8f1574 100644 --- a/rmw_iceoryx_cpp/src/rmw_publisher.cpp +++ b/rmw_iceoryx_cpp/src/rmw_publisher.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -74,7 +75,7 @@ rmw_create_publisher( std::string node_full_name = std::string(node->namespace_) + std::string(node->name); rmw_publisher_t * rmw_publisher = nullptr; - iox::popo::Publisher * iceoryx_sender = nullptr; + iox::popo::UntypedPublisher * iceoryx_sender = nullptr; IceoryxPublisher * iceoryx_publisher = nullptr; // allocate rmw_publisher @@ -86,8 +87,8 @@ rmw_create_publisher( // allocate iceoryx_sender iceoryx_sender = - static_cast(rmw_allocate( - sizeof(iox::popo::Publisher))); + static_cast(rmw_allocate( + sizeof(iox::popo::UntypedPublisher))); if (!iceoryx_sender) { RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx sender"); goto fail; @@ -95,8 +96,10 @@ rmw_create_publisher( RMW_TRY_PLACEMENT_NEW( iceoryx_sender, iceoryx_sender, - goto fail, iox::popo::Publisher, service_description, - iox::cxx::CString100(iox::cxx::TruncateToCapacity, node_full_name)); + goto fail, iox::popo::UntypedPublisher, service_description, + iox::popo::PublisherOptions{ + 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); + iceoryx_sender->offer(); // make the sender visible // allocate iceoryx_publisher @@ -128,7 +131,7 @@ rmw_create_publisher( if (rmw_publisher) { if (iceoryx_sender) { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - iceoryx_sender->~Publisher(), iox::popo::Publisher) + iceoryx_sender->~UntypedPublisherImpl(), iox::popo::UntypedPublisher) rmw_free(iceoryx_sender); } if (iceoryx_publisher) { @@ -162,7 +165,7 @@ rmw_publisher_get_actual_qos(const rmw_publisher_t * publisher, rmw_qos_profile_ (void)publisher; - // TODO(mphnl) check in detail + /// @todo poehnl: check in detail *qos = rmw_qos_profile_default; return RMW_RET_OK; @@ -185,7 +188,7 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) if (iceoryx_publisher) { if (iceoryx_publisher->iceoryx_sender_) { RMW_TRY_DESTRUCTOR( - iceoryx_publisher->iceoryx_sender_->~Publisher(), + iceoryx_publisher->iceoryx_sender_->~UntypedPublisherImpl(), iceoryx_publisher->iceoryx_sender_, result = RMW_RET_ERROR) rmw_free(iceoryx_publisher->iceoryx_sender_); diff --git a/rmw_iceoryx_cpp/src/rmw_request.cpp b/rmw_iceoryx_cpp/src/rmw_request.cpp index 0f77eed..0dbbb76 100644 --- a/rmw_iceoryx_cpp/src/rmw_request.cpp +++ b/rmw_iceoryx_cpp/src/rmw_request.cpp @@ -19,6 +19,7 @@ extern "C" { +/// @todo Use the new request/response API of iceoryx v2.0 here rmw_ret_t rmw_send_request( const rmw_client_t * client, @@ -33,6 +34,7 @@ rmw_send_request( return RMW_RET_UNSUPPORTED; } +/// @todo Use the new request/response API of iceoryx v2.0 here rmw_ret_t rmw_take_request( const rmw_service_t * service, diff --git a/rmw_iceoryx_cpp/src/rmw_response.cpp b/rmw_iceoryx_cpp/src/rmw_response.cpp index e73debe..3eb2ab2 100644 --- a/rmw_iceoryx_cpp/src/rmw_response.cpp +++ b/rmw_iceoryx_cpp/src/rmw_response.cpp @@ -20,6 +20,8 @@ extern "C" { rmw_ret_t + +/// @todo Use the new request/response API of iceoryx v2.0 here rmw_take_response( const rmw_client_t * client, rmw_service_info_t * request_header, @@ -35,6 +37,7 @@ rmw_take_response( return RMW_RET_UNSUPPORTED; } +/// @todo Use the new request/response API of iceoryx v2.0 here rmw_ret_t rmw_send_response( const rmw_service_t * service, diff --git a/rmw_iceoryx_cpp/src/rmw_service.cpp b/rmw_iceoryx_cpp/src/rmw_service.cpp index f26a16a..8d8be53 100644 --- a/rmw_iceoryx_cpp/src/rmw_service.cpp +++ b/rmw_iceoryx_cpp/src/rmw_service.cpp @@ -20,6 +20,7 @@ extern "C" { +/// @todo Use the new request/response API of iceoryx v2.0 here instead of dummy services rmw_service_t * rmw_create_service( const rmw_node_t * node, @@ -32,8 +33,6 @@ rmw_create_service( RCUTILS_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr); RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); - // TODO(mphnl) dummy for now for being able to continue with pub/sub - // Has to be realized later with a sender and receiver pair of iceoryx rmw_service_t * rmw_service = nullptr; rmw_service = rmw_service_allocate(); diff --git a/rmw_iceoryx_cpp/src/rmw_subscription.cpp b/rmw_iceoryx_cpp/src/rmw_subscription.cpp index 83c873c..11f30cc 100644 --- a/rmw_iceoryx_cpp/src/rmw_subscription.cpp +++ b/rmw_iceoryx_cpp/src/rmw_subscription.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -75,7 +76,7 @@ rmw_create_subscription( std::string node_full_name = std::string(node->namespace_) + std::string(node->name); rmw_subscription_t * rmw_subscription = nullptr; - iox::popo::Subscriber * iceoryx_receiver = nullptr; + iox::popo::UntypedSubscriber * iceoryx_receiver = nullptr; IceoryxSubscription * iceoryx_subscription = nullptr; rmw_subscription = rmw_subscription_allocate(); @@ -85,18 +86,20 @@ rmw_create_subscription( } iceoryx_receiver = - static_cast(rmw_allocate( - sizeof(iox::popo::Subscriber))); + static_cast(rmw_allocate( + sizeof(iox::popo::UntypedSubscriber))); if (!iceoryx_receiver) { RMW_SET_ERROR_MSG("failed to allocate memory for iceoryx receiver"); goto fail; } RMW_TRY_PLACEMENT_NEW( iceoryx_receiver, iceoryx_receiver, goto fail, - iox::popo::Subscriber, service_description, - iox::cxx::CString100(iox::cxx::TruncateToCapacity, node_full_name)) + iox::popo::UntypedSubscriber, service_description, + iox::popo::SubscriberOptions{ + qos_policies->depth, 0U, iox::NodeName_t(iox::cxx::TruncateToCapacity, node_full_name)}); + // instant subscribe, queue size form qos settings - iceoryx_receiver->subscribe(qos_policies->depth); + iceoryx_receiver->subscribe(); iceoryx_subscription = static_cast(rmw_allocate(sizeof(IceoryxSubscription))); @@ -125,8 +128,9 @@ rmw_create_subscription( fail: if (rmw_subscription) { if (iceoryx_receiver) { + /// @todo Can we avoid to use the impl here? RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - iceoryx_receiver->~Subscriber(), iox::popo::Subscriber) + iceoryx_receiver->~UntypedSubscriberImpl(), iox::popo::UntypedSubscriber) rmw_free(iceoryx_receiver); } if (iceoryx_subscription) { @@ -151,7 +155,7 @@ rmw_subscription_get_actual_qos( RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); RCUTILS_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_ERROR); - // TODO(mphnl) check in detail + /// @todo poehnl: check in detail *qos = rmw_qos_profile_default; return RMW_RET_OK; @@ -176,8 +180,9 @@ rmw_destroy_subscription( static_cast(subscription->data); if (iceoryx_subscription) { if (iceoryx_subscription->iceoryx_receiver_) { + // @todo Can we avoid to use the impl here? RMW_TRY_DESTRUCTOR( - iceoryx_subscription->iceoryx_receiver_->~Subscriber(), + iceoryx_subscription->iceoryx_receiver_->~UntypedSubscriberImpl(), iceoryx_subscription->iceoryx_receiver_, result = RMW_RET_ERROR) rmw_free(iceoryx_subscription->iceoryx_receiver_); diff --git a/rmw_iceoryx_cpp/src/rmw_take.cpp b/rmw_iceoryx_cpp/src/rmw_take.cpp index 5aa4cf4..00748ca 100644 --- a/rmw_iceoryx_cpp/src/rmw_take.cpp +++ b/rmw_iceoryx_cpp/src/rmw_take.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,7 +15,7 @@ #include "./types/iceoryx_subscription.hpp" -#include "iceoryx_posh/popo/subscriber.hpp" +#include "iceoryx_posh/popo/untyped_subscriber.hpp" #include "rcutils/error_handling.h" @@ -66,7 +67,7 @@ rmw_take( } // Subscription is not matched - if (iox::popo::SubscriptionState::SUBSCRIBED != iceoryx_receiver->getSubscriptionState()) { + if (iox::SubscribeState::SUBSCRIBED != iceoryx_receiver->getSubscriptionState()) { return RMW_RET_OK; } @@ -77,27 +78,44 @@ rmw_take( } const iox::mepoo::ChunkHeader * chunk_header = nullptr; - if (!iceoryx_receiver->getChunk(&chunk_header)) { - RMW_SET_ERROR_MSG("No chunk in iceoryx_receiver"); - return RMW_RET_ERROR; + const void * user_payload = nullptr; + + rmw_ret_t ret = RMW_RET_ERROR; + iceoryx_receiver->take() + .and_then( + [&](const void * userPayload) { + user_payload = userPayload; + chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); + ret = RMW_RET_OK; + }) + .or_else( + [&](iox::popo::ChunkReceiveResult) { + RMW_SET_ERROR_MSG("No chunk in iceoryx_receiver"); + ret = RMW_RET_ERROR; + }); + + /// @todo move this to lambda in a function? + if (ret == RMW_RET_ERROR) { + return ret; } // if fixed size, we fetch the data via memcpy if (iceoryx_subscription->is_fixed_size_) { - memcpy(ros_message, chunk_header->payload(), chunk_header->m_info.m_payloadSize); - iceoryx_receiver->releaseChunk(chunk_header); + memcpy(ros_message, user_payload, chunk_header->userPayloadSize()); + iceoryx_receiver->release(user_payload); *taken = true; - return RMW_RET_OK; + ret = RMW_RET_OK; + } else { + rmw_iceoryx_cpp::deserialize( + static_cast(user_payload), + &iceoryx_subscription->type_supports_, + ros_message); + iceoryx_receiver->release(user_payload); + *taken = true; + ret = RMW_RET_OK; } - rmw_iceoryx_cpp::deserialize( - static_cast(chunk_header->payload()), - &iceoryx_subscription->type_supports_, - ros_message); - iceoryx_receiver->releaseChunk(chunk_header); - *taken = true; - - return RMW_RET_OK; + return ret; } rmw_ret_t @@ -114,7 +132,7 @@ rmw_take_with_info( RCUTILS_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_ERROR); (void)allocation; - // TODO(mphnl) implement message_info related stuff + /// @todo poehnl: implement message_info related stuff (void)message_info; return rmw_take(subscription, ros_message, taken, allocation); } @@ -151,7 +169,7 @@ rmw_take_serialized_message( } // Subscription is not matched - if (iox::popo::SubscriptionState::SUBSCRIBED != iceoryx_receiver->getSubscriptionState()) { + if (iox::SubscribeState::SUBSCRIBED != iceoryx_receiver->getSubscriptionState()) { return RMW_RET_OK; } @@ -162,17 +180,27 @@ rmw_take_serialized_message( } const iox::mepoo::ChunkHeader * chunk_header = nullptr; - if (!iceoryx_receiver->getChunk(&chunk_header)) { - RMW_SET_ERROR_MSG("No chunk in iceoryx_receiver"); - return RMW_RET_ERROR; - } + const void * user_payload = nullptr; + + rmw_ret_t ret = RMW_RET_OK; + iceoryx_receiver->take() + .and_then( + [&](const void * userPayload) { + user_payload = userPayload; + chunk_header = iox::mepoo::ChunkHeader::fromUserPayload(user_payload); + }) + .or_else( + [&](iox::popo::ChunkReceiveResult) { + RMW_SET_ERROR_MSG("No chunk in iceoryx_receiver"); + ret = RMW_RET_ERROR; + }); // all incoming data is serialzed already in memory, so simply call memcopy - auto ret = rmw_serialized_message_resize(serialized_message, chunk_header->m_info.m_payloadSize); + ret = rmw_serialized_message_resize(serialized_message, chunk_header->userPayloadSize()); if (RMW_RET_OK == ret) { - memcpy(serialized_message->buffer, chunk_header->payload(), chunk_header->m_info.m_payloadSize); - serialized_message->buffer_length = chunk_header->m_info.m_payloadSize; - iceoryx_receiver->releaseChunk(chunk_header); + memcpy(serialized_message->buffer, user_payload, chunk_header->userPayloadSize()); + serialized_message->buffer_length = chunk_header->userPayloadSize(); + iceoryx_receiver->release(user_payload); *taken = true; } @@ -193,7 +221,7 @@ rmw_take_serialized_message_with_info( RCUTILS_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_ERROR); (void)allocation; - // TODO(mphnl) implement message_info related stuff + /// @todo poehnl: implement message_info related stuff (void) message_info; return rmw_take_serialized_message(subscription, serialized_message, taken, allocation); } @@ -238,15 +266,23 @@ rmw_take_loaned_message( } if (!iceoryx_subscription->is_fixed_size_) { - // TODO(Karsten1987) Alternatively fall back to regular rmw_take with memcpy + /// @todo Karsten1987: Alternatively fall back to regular rmw_take with memcpy RMW_SET_ERROR_MSG("iceoryx can't take loaned non-fixed size data stuctures"); return RMW_RET_ERROR; } - if (!iceoryx_receiver->getChunk(const_cast(loaned_message))) { - RMW_SET_ERROR_MSG("No chunk in iceoryx_receiver"); - return RMW_RET_ERROR; - } + rmw_ret_t ret = RMW_RET_OK; + iceoryx_receiver->take() + .and_then( + [&](const void * userPayload) { + *loaned_message = const_cast(userPayload); + ret = RMW_RET_OK; + }) + .or_else( + [&](auto &) { + RMW_SET_ERROR_MSG("No chunk in iceoryx_receiver"); + ret = RMW_RET_ERROR; + }); *taken = true; return RMW_RET_OK; @@ -290,8 +326,8 @@ rmw_return_loaned_message_from_subscription( return RMW_RET_ERROR; } - auto ret = iceoryx_receiver->releaseChunk(loaned_message); - return ret ? RMW_RET_OK : RMW_RET_ERROR; + iceoryx_receiver->release(loaned_message); + return RMW_RET_OK; } rmw_ret_t diff --git a/rmw_iceoryx_cpp/src/rmw_trigger_guard_condition.cpp b/rmw_iceoryx_cpp/src/rmw_trigger_guard_condition.cpp index f84e02c..3f43b3a 100644 --- a/rmw_iceoryx_cpp/src/rmw_trigger_guard_condition.cpp +++ b/rmw_iceoryx_cpp/src/rmw_trigger_guard_condition.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./types/iceoryx_guard_condition.hpp" +#include "iceoryx_posh/popo/user_trigger.hpp" #include "rcutils/error_handling.h" @@ -33,7 +34,7 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition_handle rmw_get_implementation_identifier(), return RMW_RET_ERROR); - auto guard_condition = static_cast(guard_condition_handle->data); + auto guard_condition = static_cast(guard_condition_handle->data); if (!guard_condition) { RMW_SET_ERROR_MSG("guard condition is null"); return RMW_RET_ERROR; diff --git a/rmw_iceoryx_cpp/src/rmw_wait.cpp b/rmw_iceoryx_cpp/src/rmw_wait.cpp index fec9ec9..68cb369 100644 --- a/rmw_iceoryx_cpp/src/rmw_wait.cpp +++ b/rmw_iceoryx_cpp/src/rmw_wait.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,16 +15,16 @@ #include -#include "iceoryx_posh/popo/subscriber.hpp" +#include "iceoryx_posh/popo/untyped_subscriber.hpp" +#include "iceoryx_posh/popo/wait_set.hpp" +#include "iceoryx_posh/popo/user_trigger.hpp" #include "rcutils/error_handling.h" #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" -#include "./types/iceoryx_guard_condition.hpp" #include "./types/iceoryx_subscription.hpp" -#include "./types/iceoryx_wait_set.hpp" extern "C" { @@ -49,86 +50,66 @@ rmw_wait( : waitset, wait_set->implementation_identifier, rmw_get_implementation_identifier(), return RMW_RET_ERROR); - auto iceoryx_wait_set = static_cast(wait_set->data); - if (!iceoryx_wait_set) { + iox::popo::WaitSet * waitset = + static_cast *>(wait_set->data); + if (!waitset) { return RMW_RET_ERROR; } - iox::posix::Semaphore * semaphore = iceoryx_wait_set->semaphore_; - if (!semaphore) { - return RMW_RET_ERROR; - } - - // attach semaphore to all iceoryx receivers + bool skip_wait{false}; + // attach all iceoryx subscriber to WaitSet for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { auto iceoryx_subscription = - static_cast(subscriptions->subscribers[i]); + static_cast(subscriptions->subscribers[i]); auto iceoryx_receiver = iceoryx_subscription->iceoryx_receiver_; - // indicate that we do not have to wait if there is already a new sample - if (iceoryx_receiver->hasNewChunks()) { - goto after_wait; - } - - iceoryx_receiver->setChunkReceiveSemaphore(semaphore); + waitset->attachState(*iceoryx_receiver, iox::popo::SubscriberState::HAS_DATA).or_else( + [&](auto &) { + RMW_SET_ERROR_MSG("failed to attach subscriber"); + skip_wait = true; + }); } - // attach semaphore to all guard conditions + + // attach all guard conditions to WaitSet for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { auto iceoryx_guard_condition = - static_cast(guard_conditions->guard_conditions[i]); + static_cast(guard_conditions->guard_conditions[i]); - // indicate that we do not have to wait if there is already a triggered guard condition - if (iceoryx_guard_condition->hasTriggered()) { - goto after_wait; - } + waitset->attachEvent(*iceoryx_guard_condition).or_else( + [&](auto) { + RMW_SET_ERROR_MSG("failed to attach guard condition"); + skip_wait = true; + }); + } - iceoryx_guard_condition->attachSemaphore(semaphore); + if (skip_wait) { + goto after_wait; } if (!wait_timeout) { - semaphore->wait(); + /// @todo Check triggered subscribers in vector? Is that relevant for rmw? + auto notificationVector = waitset->wait(); } else { - struct timespec ts_start; - struct timespec ts_end; - clock_gettime(CLOCK_REALTIME, &ts_start); - uint64_t nsec = ts_start.tv_nsec + wait_timeout->nsec; - if (nsec >= 1000000000L) { - nsec -= 1000000000L; - ts_end.tv_sec = ts_start.tv_sec + wait_timeout->sec + 1; - } else { - ts_end.tv_sec = ts_start.tv_sec + wait_timeout->sec; - } - ts_end.tv_nsec = nsec; - semaphore->timedWait(&ts_end, true); - - // // for debugging - // struct timespec ts_diff; - // clock_gettime(CLOCK_REALTIME, &ts_diff); - // int64_t diff_nsec = ts_diff.tv_nsec - ts_start.tv_nsec; - // if (diff_nsec < 0L) { - // diff_nsec += 1000000000L; - // ts_diff.tv_sec = ts_diff.tv_sec - ts_start.tv_sec - 1; - // } else { - // ts_diff.tv_sec = ts_diff.tv_sec - ts_start.tv_sec; - // } - // ts_diff.tv_nsec = diff_nsec; - // printf("waited s %lu ns %lu\n", ts_diff.tv_sec, ts_diff.tv_nsec); + auto sec = iox::units::Duration::fromSeconds(wait_timeout->sec); + auto nsec = iox::units::Duration::fromNanoseconds(wait_timeout->nsec); + auto timeout = sec + nsec; + + /// @todo Check triggered subscribers in vector? Is that relevant for rmw? + auto notificationVector = waitset->timedWait(iox::units::Duration(timeout)); } after_wait: - - // reset all the subscriptions that don't have new data for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { auto iceoryx_subscription = - static_cast(subscriptions->subscribers[i]); - iox::popo::Subscriber * iceoryx_receiver = iceoryx_subscription->iceoryx_receiver_; + static_cast(subscriptions->subscribers[i]); + iox::popo::UntypedSubscriber * iceoryx_receiver = iceoryx_subscription->iceoryx_receiver_; - // remove semaphore from all receivers because next call a new waitset could be provided - iceoryx_receiver->unsetChunkReceiveSemaphore(); + // remove waitset from all receivers because next call a new waitset could be provided + waitset->detachState(*iceoryx_receiver, iox::popo::SubscriberState::HAS_DATA); - if (!iceoryx_receiver->hasNewChunks()) { + if (!iceoryx_receiver->hasData()) { subscriptions->subscribers[i] = nullptr; } } @@ -136,22 +117,15 @@ rmw_wait( // reset all the guard_conditions that have not triggered for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { auto iceoryx_guard_condition = - static_cast(guard_conditions->guard_conditions[i]); + static_cast(guard_conditions->guard_conditions[i]); - iceoryx_guard_condition->detachSemaphore(); + waitset->detachEvent(*iceoryx_guard_condition); - if (iceoryx_guard_condition->hasTriggered()) { - iceoryx_guard_condition->resetTriggerIndication(); - } else { + if (!iceoryx_guard_condition->hasTriggered()) { guard_conditions->guard_conditions[i] = nullptr; } } - // clear the semaphore - // events that triggered it and where not yet collected will be seen on next rmw_wait - while (semaphore->tryWait()) { - } - return RMW_RET_OK; } } // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_wait_set.cpp b/rmw_iceoryx_cpp/src/rmw_wait_set.cpp index 07067d5..7e3c928 100644 --- a/rmw_iceoryx_cpp/src/rmw_wait_set.cpp +++ b/rmw_iceoryx_cpp/src/rmw_wait_set.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,9 +14,9 @@ // limitations under the License. #include "./iceoryx_identifier.hpp" -#include "./types/iceoryx_wait_set.hpp" #include "iceoryx_posh/roudi/introspection_types.hpp" +#include "iceoryx_posh/popo/wait_set.hpp" #include "rcutils/error_handling.h" @@ -35,12 +36,8 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) : context, context->implementation_identifier, rmw_get_implementation_identifier(), return nullptr); - // Untill we have something better in the iceoryx API, - // we use the process introspection and its semaphore for notification in the ros waitset rmw_wait_set_t * rmw_wait_set = nullptr; - iox::popo::Subscriber * process_receiver = nullptr; - iox::posix::Semaphore * semaphore = nullptr; - IceoryxWaitSet * iceoryx_wait_set = nullptr; + iox::popo::WaitSet * waitset = nullptr; rmw_wait_set = rmw_wait_set_allocate(); if (!rmw_wait_set) { @@ -50,49 +47,29 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) rmw_wait_set->implementation_identifier = rmw_get_implementation_identifier(); - process_receiver = static_cast( - rmw_allocate(sizeof(iox::popo::Subscriber))); - if (!process_receiver) { + // create waitset + waitset = static_cast *>( + rmw_allocate(sizeof(iox::popo::WaitSet))); + if (!waitset) { RMW_SET_ERROR_MSG("failed to allocate memory for wait_set data"); goto fail; } RMW_TRY_PLACEMENT_NEW( - process_receiver, - process_receiver, + waitset, + waitset, goto fail, - iox::popo::Subscriber, - iox::roudi::IntrospectionProcessService) + iox::popo::WaitSet); - semaphore = process_receiver->getSemaphore(); - - // Set semaphore and subscribe for having wait_set triggers if a new process appears - process_receiver->setChunkReceiveSemaphore(semaphore); - process_receiver->subscribe(1); - - iceoryx_wait_set = static_cast(rmw_allocate(sizeof(IceoryxWaitSet))); - if (!iceoryx_wait_set) { - RMW_SET_ERROR_MSG("failed to allocate memory for rmw iceoryx wait_set"); - goto fail; - } - RMW_TRY_PLACEMENT_NEW( - iceoryx_wait_set, iceoryx_wait_set, goto fail, IceoryxWaitSet, semaphore, process_receiver) - - rmw_wait_set->data = static_cast(iceoryx_wait_set); + rmw_wait_set->data = static_cast(waitset); return rmw_wait_set; fail: if (rmw_wait_set) { - if (process_receiver) { + if (waitset) { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - process_receiver->~Subscriber(), - iox::popo::Subscriber) - rmw_free(process_receiver); - } - - if (iceoryx_wait_set) { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - iceoryx_wait_set->~IceoryxWaitSet(), IceoryxWaitSet) - rmw_free(iceoryx_wait_set); + waitset->~WaitSet(), + iox::popo::WaitSet) + rmw_free(waitset); } rmw_wait_set_free(rmw_wait_set); @@ -112,17 +89,14 @@ rmw_destroy_wait_set(rmw_wait_set_t * wait_set) rmw_ret_t result = RMW_RET_OK; - auto iceoryx_wait_set = static_cast(wait_set->data); + auto iceoryx_wait_set = + static_cast *>(wait_set->data); + if (iceoryx_wait_set) { - if (iceoryx_wait_set->iceoryx_receiver_) { - RMW_TRY_DESTRUCTOR( - iceoryx_wait_set->iceoryx_receiver_->~Subscriber(), - iceoryx_wait_set->iceoryx_receiver_, - result = RMW_RET_ERROR) - rmw_free(iceoryx_wait_set->iceoryx_receiver_); - } RMW_TRY_DESTRUCTOR( - iceoryx_wait_set->~IceoryxWaitSet(), iceoryx_wait_set, result = RMW_RET_ERROR) + iceoryx_wait_set->~WaitSet(), + iceoryx_wait_set, + result = RMW_RET_ERROR) rmw_free(iceoryx_wait_set); } wait_set->data = nullptr; diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_guard_condition.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_guard_condition.hpp deleted file mode 100644 index 6f58cb9..0000000 --- a/rmw_iceoryx_cpp/src/types/iceoryx_guard_condition.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TYPES__ICEORYX_GUARD_CONDITION_HPP_ -#define TYPES__ICEORYX_GUARD_CONDITION_HPP_ - -#include -#include - -#include "iceoryx_utils/posix_wrapper/semaphore.hpp" - -class IceoryxGuardCondition -{ -public: - IceoryxGuardCondition() - : semaphore_(nullptr), triggered_(false) {} - - void - trigger() - { - { - std::lock_guard lock(mutex_); - if (semaphore_ != nullptr) { - semaphore_->post(); - } - } - - triggered_.store(true, std::memory_order_relaxed); - } - - void - attachSemaphore(iox::posix::Semaphore * const semaphore) - { - std::lock_guard lock(mutex_); - semaphore_ = semaphore; - } - - void - detachSemaphore() - { - std::lock_guard lock(mutex_); - semaphore_ = nullptr; - } - - bool - hasTriggered() - { - return triggered_.load(std::memory_order_relaxed); - } - - void resetTriggerIndication() - { - triggered_.store(false, std::memory_order_relaxed); - } - -private: - std::mutex mutex_; - iox::posix::Semaphore * semaphore_; - std::atomic_bool triggered_; -}; - -#endif // TYPES__ICEORYX_GUARD_CONDITION_HPP_ diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_node.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_node.hpp index 2fd5fa3..f7cdaa8 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_node.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_node.hpp @@ -17,9 +17,10 @@ #include -#include "iceoryx_posh/popo/subscriber.hpp" +#include "iceoryx_posh/popo/untyped_subscriber.hpp" #include "iceoryx_posh/roudi/introspection_types.hpp" -#include "iceoryx_posh/runtime/runnable.hpp" +#include "iceoryx_posh/runtime/node.hpp" +#include "iceoryx_posh/popo/listener.hpp" #include "rcutils/error_handling.h" @@ -27,12 +28,12 @@ #include "rmw/rmw.h" #include "../iceoryx_identifier.hpp" -#include "./iceoryx_guard_condition.hpp" +#include "iceoryx_posh/popo/user_trigger.hpp" // We currently use the iceoryx port introspection // which is updated whenever a sender port comes or goes or // does OFFER / STOP_OFFER or a receiver port comes or goes or does SUB / UNSUB -// TODO(mphnl) check with the list of ros2 graph events +/// @todo poehnl: check with the list of ros2 graph events class IceoryxGraphChangeNotifier { @@ -43,7 +44,7 @@ class IceoryxGraphChangeNotifier RMW_SET_ERROR_MSG("invalid input for GraphChangeNotifier"); iceoryx_guard_condition_ = nullptr; } else { - iceoryx_guard_condition_ = static_cast(guard_condition->data); + iceoryx_guard_condition_ = static_cast(guard_condition->data); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( IceoryxGraphChangeNotifier : guard_condition, @@ -52,28 +53,46 @@ class IceoryxGraphChangeNotifier iceoryx_guard_condition_ = nullptr); } + /// @todo change to the dds_common graph // subscribe with a callback for changes in the iceoryx graph - port_receiver_.setReceiveHandler(std::bind(&IceoryxGraphChangeNotifier::callback, this)); - port_receiver_.subscribe(1); + // https://github.com/eclipse-iceoryx/iceoryx/issues/707 + listener_.attachEvent( + port_receiver_, iox::popo::SubscriberEvent::DATA_RECEIVED, + iox::popo::createNotificationCallback(IceoryxGraphChangeNotifier::callback, *this)) + .or_else( + [](auto) { + RMW_SET_ERROR_MSG("unable to attach port_receiver_"); + std::terminate(); + }); + port_receiver_.subscribe(); } ~IceoryxGraphChangeNotifier() { - port_receiver_.unsetReceiveHandler(); + listener_.detachEvent(port_receiver_, iox::popo::SubscriberEvent::DATA_RECEIVED); port_receiver_.unsubscribe(); } private: - void callback() + // must be a static method to be convertable to c function pointer + // second argument (self) is the this pointer of the current object + static void callback( + iox::popo::UntypedSubscriber * introspectionSubscriber, + IceoryxGraphChangeNotifier * self) { - if (nullptr != iceoryx_guard_condition_) { - iceoryx_guard_condition_->trigger(); + if (nullptr != self->iceoryx_guard_condition_) { + self->iceoryx_guard_condition_->trigger(); + } + if (nullptr != introspectionSubscriber) { + introspectionSubscriber->releaseQueuedData(); } } - - IceoryxGuardCondition * iceoryx_guard_condition_{nullptr}; - using port_receiver_t = iox::popo::Subscriber; - port_receiver_t port_receiver_{iox::roudi::IntrospectionPortService}; + iox::popo::UserTrigger * iceoryx_guard_condition_{nullptr}; + using port_receiver_t = iox::popo::UntypedSubscriber; + port_receiver_t port_receiver_{iox::roudi::IntrospectionPortService, + iox::popo::SubscriberOptions{1U, 1U, "", true}}; + using listener_t = iox::popo::Listener; + listener_t listener_; }; struct IceoryxNodeInfo @@ -81,7 +100,7 @@ struct IceoryxNodeInfo IceoryxNodeInfo( rmw_guard_condition_t * guard_condition, IceoryxGraphChangeNotifier * graph_change_notifier, - iox::runtime::Runnable * iceoryx_runnable) + iox::runtime::Node * iceoryx_runnable) : guard_condition_(guard_condition), graph_change_notifier_(graph_change_notifier), iceoryx_runnable_(iceoryx_runnable) @@ -89,7 +108,7 @@ struct IceoryxNodeInfo } rmw_guard_condition_t * const guard_condition_; IceoryxGraphChangeNotifier * const graph_change_notifier_; - iox::runtime::Runnable * const iceoryx_runnable_; + iox::runtime::Node * const iceoryx_runnable_; }; #endif // TYPES__ICEORYX_NODE_HPP_ diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_publisher.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_publisher.hpp index 225a5c5..813e518 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_publisher.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_publisher.hpp @@ -17,7 +17,7 @@ #include "../iceoryx_generate_gid.hpp" -#include "iceoryx_posh/popo/publisher.hpp" +#include "iceoryx_posh/popo/untyped_publisher.hpp" #include "rmw/rmw.h" #include "rmw/types.h" @@ -28,7 +28,7 @@ struct IceoryxPublisher { IceoryxPublisher( const rosidl_message_type_support_t * type_supports, - iox::popo::Publisher * const iceoryx_sender) + iox::popo::UntypedPublisher * const iceoryx_sender) : type_supports_(*type_supports), iceoryx_sender_(iceoryx_sender), gid_(generate_gid()), @@ -37,7 +37,7 @@ struct IceoryxPublisher {} rosidl_message_type_support_t type_supports_; - iox::popo::Publisher * const iceoryx_sender_; + iox::popo::UntypedPublisher * const iceoryx_sender_; rmw_gid_t gid_; bool is_fixed_size_; size_t message_size_; diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_subscription.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_subscription.hpp index 67641c6..9ff2add 100644 --- a/rmw_iceoryx_cpp/src/types/iceoryx_subscription.hpp +++ b/rmw_iceoryx_cpp/src/types/iceoryx_subscription.hpp @@ -15,7 +15,7 @@ #ifndef TYPES__ICEORYX_SUBSCRIPTION_HPP_ #define TYPES__ICEORYX_SUBSCRIPTION_HPP_ -#include "iceoryx_posh/popo/subscriber.hpp" +#include "iceoryx_posh/popo/untyped_subscriber.hpp" #include "rmw/rmw.h" #include "rmw/types.h" @@ -26,7 +26,7 @@ struct IceoryxSubscription { IceoryxSubscription( const rosidl_message_type_support_t * type_supports, - iox::popo::Subscriber * const iceoryx_receiver) + iox::popo::UntypedSubscriber * const iceoryx_receiver) : type_supports_(*type_supports), iceoryx_receiver_(iceoryx_receiver), is_fixed_size_(rmw_iceoryx_cpp::iceoryx_is_fixed_size(type_supports)), @@ -34,7 +34,7 @@ struct IceoryxSubscription {} rosidl_message_type_support_t type_supports_; - iox::popo::Subscriber * const iceoryx_receiver_; + iox::popo::UntypedSubscriber * const iceoryx_receiver_; bool is_fixed_size_; size_t message_size_; }; diff --git a/rmw_iceoryx_cpp/src/types/iceoryx_wait_set.hpp b/rmw_iceoryx_cpp/src/types/iceoryx_wait_set.hpp deleted file mode 100644 index 7c4f8f7..0000000 --- a/rmw_iceoryx_cpp/src/types/iceoryx_wait_set.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TYPES__ICEORYX_WAIT_SET_HPP_ -#define TYPES__ICEORYX_WAIT_SET_HPP_ - -#include "iceoryx_posh/popo/subscriber.hpp" -#include "iceoryx_utils/posix_wrapper/semaphore.hpp" - -#include "rmw/rmw.h" -#include "rmw/types.h" - -struct IceoryxWaitSet -{ - IceoryxWaitSet( - iox::posix::Semaphore * const semaphore, - iox::popo::Subscriber * const iceoryx_receiver) - : semaphore_(semaphore), iceoryx_receiver_(iceoryx_receiver) - {} - - iox::posix::Semaphore * const semaphore_; - iox::popo::Subscriber * const iceoryx_receiver_; -}; - -#endif // TYPES__ICEORYX_WAIT_SET_HPP_ diff --git a/rmw_iceoryx_cpp/test/test_msgs_c_fixtures.hpp b/rmw_iceoryx_cpp/test/test_msgs_c_fixtures.hpp index 8b6b469..ccf4d34 100644 --- a/rmw_iceoryx_cpp/test/test_msgs_c_fixtures.hpp +++ b/rmw_iceoryx_cpp/test/test_msgs_c_fixtures.hpp @@ -975,7 +975,8 @@ bool operator!=(const test_msgs__msg__Nested & lhs, const test_msgs__msg__Nested return !(lhs == rhs); } -// TODO(karsten1987): This requires some sort of deep-copy operator for arrays and sequences, +/// @todo karsten1987 +// This requires some sort of deep-copy operator for arrays and sequences, // especially their strings as the shared ptr will cleanup the strings // and eventually lead to memory leaks. /* From f5d4026dd8fc0547af792a0334acc053ae708cec Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Tue, 27 Jul 2021 15:44:19 +0200 Subject: [PATCH 2/9] #48 Remove additional repos file as iceoryx is now available in upstream ROS 2 Signed-off-by: Simon Hoinkis --- .github/workflows/additional_repos.repos | 5 ----- .github/workflows/main.yml | 4 ++-- README.md | 4 ++-- 3 files changed, 4 insertions(+), 9 deletions(-) delete mode 100644 .github/workflows/additional_repos.repos diff --git a/.github/workflows/additional_repos.repos b/.github/workflows/additional_repos.repos deleted file mode 100644 index c7c8519..0000000 --- a/.github/workflows/additional_repos.repos +++ /dev/null @@ -1,5 +0,0 @@ -repositories: - eclipse/iceoryx: - type: git - url: https://github.com/eclipse-iceoryx/iceoryx.git - version: v1.0.1 diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 4aeb0a1..c7b8c8f 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -18,11 +18,11 @@ jobs: - name: Setup ROS uses: ros-tooling/setup-ros@master with: - required-ros-distributions: foxy + required-ros-distributions: rolling - name: Install Iceoryx Dependencies run: sudo apt-get update && sudo apt-get install -y cmake libacl1-dev libncurses5-dev pkg-config - name: Build & Test uses: ros-tooling/action-ros-ci@master with: package-name: rmw_iceoryx_cpp iceoryx_ros2_bridge - target-ros2-distro: foxy \ No newline at end of file + target-ros2-distro: rolling diff --git a/README.md b/README.md index 66c175e..02777b8 100644 --- a/README.md +++ b/README.md @@ -20,12 +20,12 @@ git clone https://github.com/ros2/rmw_iceoryx.git ``` For alternative installation instructions and more details about iceoryx's internals, please see [iceoryx's GitHub repo](https://github.com/eclipse/iceoryx). -rmw_iceoryx is compatible with ROS 2 Foxy. +rmw_iceoryx is compatible with ROS 2 Galactic. Assuming you have ROS2 installed correctly, you can compile the iceoryx workspace with colcon: ``` cd ~/iceoryx_ws/ -source /opt/ros/foxy/setup.bash # alternatively source your own ROS 2 workspace +source /opt/ros/galactic/setup.bash # alternatively source your own ROS 2 workspace colcon build # or with more options colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF From 4af92e3245899266e9586112b33780ab39f53052 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Tue, 27 Jul 2021 16:46:39 +0200 Subject: [PATCH 3/9] #48 Adhere rmw_iceoryx to galactic APIs Signed-off-by: Simon Hoinkis --- iceoryx_ros2_bridge/src/generic_subscription.cpp | 8 ++++++++ iceoryx_ros2_bridge/src/generic_subscription.hpp | 4 ++++ 2 files changed, 12 insertions(+) diff --git a/iceoryx_ros2_bridge/src/generic_subscription.cpp b/iceoryx_ros2_bridge/src/generic_subscription.cpp index 614cae7..9a4dae9 100644 --- a/iceoryx_ros2_bridge/src/generic_subscription.cpp +++ b/iceoryx_ros2_bridge/src/generic_subscription.cpp @@ -88,6 +88,14 @@ void GenericSubscription::return_serialized_message( message.reset(); } +void GenericSubscription::handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) +{ + (void) serialized_message; + (void) message_info; +} + void GenericSubscription::handle_loaned_message( void * loaned_message, const rclcpp::MessageInfo & message_info) diff --git a/iceoryx_ros2_bridge/src/generic_subscription.hpp b/iceoryx_ros2_bridge/src/generic_subscription.hpp index 1afbab5..8706d75 100644 --- a/iceoryx_ros2_bridge/src/generic_subscription.hpp +++ b/iceoryx_ros2_bridge/src/generic_subscription.hpp @@ -79,6 +79,10 @@ class GenericSubscription : public rclcpp::SubscriptionBase void return_serialized_message(std::shared_ptr & message) override; + void handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) override; + void handle_loaned_message( void * loaned_message, const rclcpp::MessageInfo & message_info) override; From d82a3c41cb3ba5eff2d0d91ba7b7d0be4a20dd7b Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 29 Jul 2021 15:17:52 +0200 Subject: [PATCH 4/9] #48 Wrap get_message_typesupport_handle(), remove duplicate error handling and reset error Signed-off-by: Simon Hoinkis --- .../iceoryx_type_info_introspection.hpp | 11 ++ .../src/internal/iceoryx_deserialize.cpp | 31 ++- .../src/internal/iceoryx_serialize.cpp | 23 +-- .../iceoryx_type_info_introspection.cpp | 177 ++++++++---------- 4 files changed, 116 insertions(+), 126 deletions(-) diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp index d9dc1ad..a0ad57b 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp @@ -22,6 +22,17 @@ struct rosidl_message_type_support_t; namespace rmw_iceoryx_cpp { +enum class TypeSupportLanguage +{ + CPP, + C +}; + +/// @brief Wraps get_message_typesupport_handle() and does error handling +/// @return std::pair containing enum TypeSupportLanguage and handle to the type support +const std::pair get_type_support( + const rosidl_message_type_support_t * type_supports); + bool iceoryx_is_fixed_size(const rosidl_message_type_support_t * type_supports); bool iceoryx_is_valid_type_support(const rosidl_message_type_support_t * type_supports); diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp index 226f0ee..d738339 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp @@ -20,7 +20,10 @@ #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rcutils/error_handling.h" + #include "rmw_iceoryx_cpp/iceoryx_deserialize.hpp" +#include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" #include "./iceoryx_deserialize_typesupport_c.hpp" #include "./iceoryx_deserialize_typesupport_cpp.hpp" @@ -33,24 +36,16 @@ void deserialize( const rosidl_message_type_support_t * type_supports, void * ros_message) { - // serialize with cpp typesupport - auto ts_cpp = get_message_typesupport_handle( - type_supports, - rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (ts_cpp != nullptr) { - auto members = - static_cast(ts_cpp->data); - rmw_iceoryx_cpp::details_cpp::deserialize(serialized_msg, members, ros_message); - } - - // serialize with c typesupport - auto ts_c = get_message_typesupport_handle( - type_supports, - rosidl_typesupport_introspection_c__identifier); - if (ts_c != nullptr) { - auto members = - static_cast(ts_c->data); - rmw_iceoryx_cpp::details_c::deserialize(serialized_msg, members, ros_message); + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { + auto members_cpp = + static_cast(ts.second->data); + rmw_iceoryx_cpp::details_cpp::deserialize(serialized_msg, members_cpp, ros_message); + } else if (ts.first == TypeSupportLanguage::C) { + auto members_c = + static_cast(ts.second->data); + rmw_iceoryx_cpp::details_c::deserialize(serialized_msg, members_c, ros_message); } } diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp index 5e8ec70..25fce24 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp @@ -22,7 +22,10 @@ #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rcutils/error_handling.h" + #include "rmw_iceoryx_cpp/iceoryx_serialize.hpp" +#include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" #include "./iceoryx_serialize_typesupport_c.hpp" #include "./iceoryx_serialize_typesupport_cpp.hpp" @@ -35,23 +38,15 @@ void serialize( const rosidl_message_type_support_t * type_supports, std::vector & payload_vector) { - // serialize with cpp typesupport - auto ts_cpp = get_message_typesupport_handle( - type_supports, - rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (ts_cpp != nullptr) { + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { auto members = - static_cast(ts_cpp->data); + static_cast(ts.second->data); rmw_iceoryx_cpp::details_cpp::serialize(ros_message, members, payload_vector); - } - - // serialize with c typesupport - auto ts_c = get_message_typesupport_handle( - type_supports, - rosidl_typesupport_introspection_c__identifier); - if (ts_c != nullptr) { + } else if (ts.first == TypeSupportLanguage::C) { auto members = - static_cast(ts_c->data); + static_cast(ts.second->data); rmw_iceoryx_cpp::details_c::serialize(ros_message, members, payload_vector); } } diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index 417acd2..b5b4a62 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "rosidl_typesupport_cpp/message_type_support.hpp" @@ -27,6 +28,8 @@ #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rcutils/error_handling.h" + #include "rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp" namespace details_cpp @@ -125,172 +128,158 @@ bool is_fixed_size(const rosidl_typesupport_introspection_c__MessageMembers * me namespace rmw_iceoryx_cpp { - -bool iceoryx_is_fixed_size(const rosidl_message_type_support_t * type_supports) +const std::pair get_type_support( + const rosidl_message_type_support_t * type_supports) { + rcutils_error_string_t cpp_error_string; + rcutils_error_string_t c_error_string; + // first, try to extract cpp type support auto ts_cpp = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); if (ts_cpp != nullptr) { - auto members = - static_cast(ts_cpp->data); - return details_cpp::is_fixed_size(members); + return std::make_pair(rmw_iceoryx_cpp::TypeSupportLanguage::CPP, ts_cpp); + } else { + /// @todo Reset error string since this is not yet an error + /// https://github.com/ros2/rosidl_typesupport/pull/102 + cpp_error_string = rcutils_get_error_string(); + rcutils_reset_error(); } // second, try to extract c type support auto ts_c = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); if (ts_c != nullptr) { - auto members = - static_cast(ts_c->data); - return details_c::is_fixed_size(members); + return std::make_pair(rmw_iceoryx_cpp::TypeSupportLanguage::C, ts_c); + } else { + /// @todo https://github.com/ros2/rosidl_typesupport/pull/102 + c_error_string = rcutils_get_error_string(); + rcutils_reset_error(); } // still here? Then something's wrong - throw std::runtime_error("no suitable type support given"); + std::stringstream error_string; + error_string << "No suitable type support given! "; + error_string << "cpp error: " << cpp_error_string.str; + error_string << "c error: " << c_error_string.str; + throw std::runtime_error(error_string.str()); } -size_t iceoryx_get_message_size(const rosidl_message_type_support_t * type_supports) +bool iceoryx_is_fixed_size(const rosidl_message_type_support_t * type_supports) { - // first, try to extract cpp type support - auto ts_cpp = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (ts_cpp != nullptr) { + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { auto members = - static_cast(ts_cpp->data); - return members->size_of_; + static_cast(ts.second->data); + return details_cpp::is_fixed_size(members); + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); + return details_c::is_fixed_size(members); } + // Something went wrong + return false; +} - // second, try to extract c type support - auto ts_c = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (ts_c != nullptr) { +size_t iceoryx_get_message_size(const rosidl_message_type_support_t * type_supports) +{ + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { auto members = - static_cast(ts_c->data); + static_cast(ts.second->data); + return members->size_of_; + } else if (ts.first == TypeSupportLanguage::C) { + auto members = + static_cast(ts.second->data); return members->size_of_; } - - // still here? Then something's wrong - throw std::runtime_error("no suitable type support given"); + // Something went wrong + return 0; } std::string iceoryx_get_message_name(const rosidl_message_type_support_t * type_supports) { - // first, try to extract cpp type support - auto ts_cpp = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (ts_cpp != nullptr) { + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { auto members = - static_cast(ts_cpp->data); + static_cast(ts.second->data); return members->message_name_; - } - - // second, try to extract c type support - auto ts_c = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (ts_c != nullptr) { + } else if (ts.first != TypeSupportLanguage::C) { auto members = - static_cast(ts_c->data); + static_cast(ts.second->data); return members->message_name_; } - - // still here? Then something's wrong - throw std::runtime_error("no suitable type support given"); + // Something went wrong + return ""; } std::string iceoryx_get_message_namespace(const rosidl_message_type_support_t * type_supports) { - // first, try to extract cpp type support - auto ts_cpp = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (ts_cpp != nullptr) { + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { auto members = - static_cast(ts_cpp->data); + static_cast(ts.second->data); return members->message_namespace_; - } - - // second, try to extract c type support - auto ts_c = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (ts_c != nullptr) { + } else if (ts.first == TypeSupportLanguage::C) { auto members = - static_cast(ts_c->data); + static_cast(ts.second->data); return members->message_namespace_; } - - // still here? Then something's wrong - throw std::runtime_error("no suitable type support given"); + // Something went wrong + return ""; } bool iceoryx_is_valid_type_support(const rosidl_message_type_support_t * type_supports) { - if (get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier)) - { - return true; + try { + get_type_support(type_supports); + } catch (...) { + return false; } - if (get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier)) - { - return true; - } - return false; + + return true; } void iceoryx_init_message( const rosidl_message_type_support_t * type_supports, void * message) { - // first, try to extract cpp type support - auto ts_cpp = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (ts_cpp != nullptr) { + auto ts = get_type_support(type_supports); + + if (ts.first == TypeSupportLanguage::CPP) { auto members = - static_cast(ts_cpp->data); + static_cast(ts.second->data); members->init_function(message, rosidl_runtime_cpp::MessageInitialization::ALL); return; - } - - // second, try to extract c type support - auto ts_c = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (ts_c != nullptr) { + } else if (ts.first == TypeSupportLanguage::C) { auto members = - static_cast(ts_c->data); + static_cast(ts.second->data); members->init_function(message, ROSIDL_RUNTIME_C_MSG_INIT_ALL); return; } - - // still here? Then something's wrong - throw std::runtime_error("no suitable type support given"); } void iceoryx_fini_message( const rosidl_message_type_support_t * type_supports, void * message) { - // first, try to extract cpp type support - auto ts_cpp = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (ts_cpp != nullptr) { + auto ts = get_type_support(type_supports); + if (ts.first == TypeSupportLanguage::CPP) { auto members = - static_cast(ts_cpp->data); + static_cast(ts.second->data); members->fini_function(message); return; - } - - // second, try to extract c type support - auto ts_c = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (ts_c != nullptr) { + } else if (ts.first == TypeSupportLanguage::C) { auto members = - static_cast(ts_c->data); + static_cast(ts.second->data); members->fini_function(message); return; } - - // still here? Then something's wrong - throw std::runtime_error("no suitable type support given"); } } // namespace rmw_iceoryx_cpp From 0507008e74748a4f7702ec943b8a107f6e8cf15c Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 29 Jul 2021 16:01:54 +0200 Subject: [PATCH 5/9] #48 Add empty implementations for new rmw galactic API calls Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/CMakeLists.txt | 2 + .../src/rmw_network_flow_endpoint.cpp | 48 +++++++++++++++++++ rmw_iceoryx_cpp/src/rmw_publisher.cpp | 10 ++++ rmw_iceoryx_cpp/src/rmw_qos.cpp | 38 +++++++++++++++ 4 files changed, 98 insertions(+) create mode 100644 rmw_iceoryx_cpp/src/rmw_network_flow_endpoint.cpp create mode 100644 rmw_iceoryx_cpp/src/rmw_qos.cpp diff --git a/rmw_iceoryx_cpp/CMakeLists.txt b/rmw_iceoryx_cpp/CMakeLists.txt index 36eec3c..666ccf6 100644 --- a/rmw_iceoryx_cpp/CMakeLists.txt +++ b/rmw_iceoryx_cpp/CMakeLists.txt @@ -87,11 +87,13 @@ add_library(rmw_iceoryx_cpp SHARED src/rmw_guard_condition.cpp src/rmw_init.cpp src/rmw_logging.cpp + src/rmw_network_flow_endpoint.cpp src/rmw_node.cpp src/rmw_node_info_and_types.cpp src/rmw_node_names.cpp src/rmw_publish.cpp src/rmw_publisher.cpp + src/rmw_qos.cpp src/rmw_request.cpp src/rmw_response.cpp src/rmw_serialize.cpp diff --git a/rmw_iceoryx_cpp/src/rmw_network_flow_endpoint.cpp b/rmw_iceoryx_cpp/src/rmw_network_flow_endpoint.cpp new file mode 100644 index 0000000..2d15c6f --- /dev/null +++ b/rmw_iceoryx_cpp/src/rmw_network_flow_endpoint.cpp @@ -0,0 +1,48 @@ +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw/get_network_flow_endpoints.h" +#include "rcutils/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" + +extern "C" +{ +rmw_ret_t +rmw_publisher_get_network_flow_endpoints( + const rmw_publisher_t * publisher, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(network_flow_endpoint_array, RMW_RET_ERROR); + + RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support get network flow endpoints."); + return RMW_RET_UNSUPPORTED; +} + +rmw_ret_t +rmw_subscription_get_network_flow_endpoints( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_ERROR); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_ERROR); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(network_flow_endpoint_array, RMW_RET_ERROR); + + RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support get network flow endpoints."); + return RMW_RET_UNSUPPORTED; +} +} // extern "C" diff --git a/rmw_iceoryx_cpp/src/rmw_publisher.cpp b/rmw_iceoryx_cpp/src/rmw_publisher.cpp index f8f1574..591806c 100644 --- a/rmw_iceoryx_cpp/src/rmw_publisher.cpp +++ b/rmw_iceoryx_cpp/src/rmw_publisher.cpp @@ -157,6 +157,16 @@ rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) return RMW_RET_UNSUPPORTED; } +rmw_ret_t +rmw_publisher_wait_for_all_acked(const rmw_publisher_t * publisher, rmw_time_t wait_timeout) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_ERROR); + (void)wait_timeout; + + RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support wait for all acked."); + return RMW_RET_UNSUPPORTED; +} + rmw_ret_t rmw_publisher_get_actual_qos(const rmw_publisher_t * publisher, rmw_qos_profile_t * qos) { diff --git a/rmw_iceoryx_cpp/src/rmw_qos.cpp b/rmw_iceoryx_cpp/src/rmw_qos.cpp new file mode 100644 index 0000000..544d6aa --- /dev/null +++ b/rmw_iceoryx_cpp/src/rmw_qos.cpp @@ -0,0 +1,38 @@ +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw/qos_profiles.h" +#include "rcutils/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" + +extern "C" +{ +rmw_ret_t +rmw_qos_profile_check_compatible( + const rmw_qos_profile_t publisher_profile, + const rmw_qos_profile_t subscription_profile, + rmw_qos_compatibility_type_t * compatibility, + char * reason, + size_t reason_size) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(compatibility, RMW_RET_ERROR); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(reason, RMW_RET_ERROR); + (void)publisher_profile; + (void)subscription_profile; + (void)reason_size; + + RMW_SET_ERROR_MSG("rmw_iceoryx_cpp does not support QoS profile check compatible"); + return RMW_RET_UNSUPPORTED; +} +} // extern "C" From 86534a3d8951e6f6212bf62e3331f5f0fb4336fd Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 29 Jul 2021 16:10:32 +0200 Subject: [PATCH 6/9] #48 Update copyright info in headers Signed-off-by: Simon Hoinkis --- iceoryx_ros2_bridge/src/generic_subscription.cpp | 1 + iceoryx_ros2_bridge/src/generic_subscription.hpp | 1 + .../include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp | 1 + rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp | 1 + rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp | 1 + .../src/internal/iceoryx_type_info_introspection.cpp | 2 ++ 6 files changed, 7 insertions(+) diff --git a/iceoryx_ros2_bridge/src/generic_subscription.cpp b/iceoryx_ros2_bridge/src/generic_subscription.cpp index 9a4dae9..b125897 100644 --- a/iceoryx_ros2_bridge/src/generic_subscription.cpp +++ b/iceoryx_ros2_bridge/src/generic_subscription.cpp @@ -1,4 +1,5 @@ // Copyright 2018, Bosch Software Innovations GmbH. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/iceoryx_ros2_bridge/src/generic_subscription.hpp b/iceoryx_ros2_bridge/src/generic_subscription.hpp index 8706d75..56cb1ca 100644 --- a/iceoryx_ros2_bridge/src/generic_subscription.hpp +++ b/iceoryx_ros2_bridge/src/generic_subscription.hpp @@ -1,4 +1,5 @@ // Copyright 2018, Bosch Software Innovations GmbH. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp index a0ad57b..5c66da1 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp index d738339..934d99f 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_deserialize.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp index 25fce24..c2fbc68 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_serialize.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index b5b4a62..f37a503 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp @@ -1,4 +1,6 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. +// Copyright (c) 2021 by Apex.AI Inc. All rights reserved. + // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 782ec5a1851dfa5f5eb63b5660c3aaa3ce42a6a9 Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 29 Jul 2021 16:23:26 +0200 Subject: [PATCH 7/9] #48 Try to fix CI build Signed-off-by: Simon Hoinkis --- iceoryx_ros2_bridge/src/generic_subscription.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/iceoryx_ros2_bridge/src/generic_subscription.hpp b/iceoryx_ros2_bridge/src/generic_subscription.hpp index 56cb1ca..1ba42c8 100644 --- a/iceoryx_ros2_bridge/src/generic_subscription.hpp +++ b/iceoryx_ros2_bridge/src/generic_subscription.hpp @@ -82,7 +82,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase void handle_serialized_message( const std::shared_ptr & serialized_message, - const rclcpp::MessageInfo & message_info) override; + const rclcpp::MessageInfo & message_info); void handle_loaned_message( void * loaned_message, const rclcpp::MessageInfo & message_info) override; From 45e99b9dd2a406dd59b71004c14b262f0fe8170a Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 29 Jul 2021 16:27:47 +0200 Subject: [PATCH 8/9] #48 Remove empty line in license header Signed-off-by: Simon Hoinkis --- rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp index f37a503..5bf6bd4 100644 --- a/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp +++ b/rmw_iceoryx_cpp/src/internal/iceoryx_type_info_introspection.cpp @@ -1,6 +1,5 @@ // Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved. // Copyright (c) 2021 by Apex.AI Inc. All rights reserved. - // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 98d28550fe28209dce57eae4e545a8e1d7614e5a Mon Sep 17 00:00:00 2001 From: Simon Hoinkis Date: Thu, 29 Jul 2021 16:38:01 +0200 Subject: [PATCH 9/9] #48 Add missing include for std::pair Signed-off-by: Simon Hoinkis --- .../include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp index 5c66da1..f9b2fae 100644 --- a/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp +++ b/rmw_iceoryx_cpp/include/rmw_iceoryx_cpp/iceoryx_type_info_introspection.hpp @@ -17,6 +17,7 @@ #define RMW_ICEORYX_CPP__ICEORYX_TYPE_INFO_INTROSPECTION_HPP_ #include +#include struct rosidl_message_type_support_t;