From 9bde78f292a2934c8272987245c37957e98caee8 Mon Sep 17 00:00:00 2001 From: Yadu Date: Tue, 5 Sep 2023 15:27:53 +0530 Subject: [PATCH 01/50] Initial commit --- LICENSE | 201 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ README.md | 1 + 2 files changed, 202 insertions(+) create mode 100644 LICENSE create mode 100644 README.md diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..261eeb9e --- /dev/null +++ b/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md new file mode 100644 index 00000000..f8e2ad93 --- /dev/null +++ b/README.md @@ -0,0 +1 @@ +# rmw_alternative \ No newline at end of file From e14da1507566c8dcd6de0a1ec8918e15b0ca7df6 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 5 Sep 2023 19:30:32 +0800 Subject: [PATCH 02/50] Add zenoh-c vendor pkg Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 55 +++++++++++++++++++ rmw_zenoh_cpp/package.xml | 21 +++++++ .../src/detail/rmw_init_options_impl.hpp | 25 +++++++++ rmw_zenoh_cpp/src/rmw_init_options.cpp | 15 +++++ zenoh_c_vendor/CMakeLists.txt | 25 +++++++++ zenoh_c_vendor/package.xml | 21 +++++++ 6 files changed, 162 insertions(+) create mode 100644 rmw_zenoh_cpp/CMakeLists.txt create mode 100644 rmw_zenoh_cpp/package.xml create mode 100644 rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp create mode 100644 rmw_zenoh_cpp/src/rmw_init_options.cpp create mode 100644 zenoh_c_vendor/CMakeLists.txt create mode 100644 zenoh_c_vendor/package.xml diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt new file mode 100644 index 00000000..1f2f9bf0 --- /dev/null +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.8) +project(rmw_zenoh_cpp) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +find_package(rcpputils REQUIRED) +find_package(rcutils REQUIRED) +find_package(rmw REQUIRED) +find_package(zenoh_c_vendor REQUIRED) +find_package(zenohc REQUIRED) + + +add_library(rmw_zenoh_cpp + src/rmw_init_options.cpp +) + +target_link_libraries(rmw_zenoh_cpp + PRIVATE + zenohc::lib +) + +configure_rmw_library(rmw_zenoh_cpp) + +target_compile_definitions(rmw_zenoh_cpp + PRIVATE + RMW_VERSION_MAJOR=${rmw_VERSION_MAJOR} + RMW_VERSION_MINOR=${rmw_VERSION_MINOR} + RMW_VERSION_PATCH=${rmw_VERSION_PATCH} +) + +ament_export_targets(export_rmw_zenoh_cpp) + +register_rmw_implementation("c:rosidl_typesupport_introspection_c" + "cpp:rosidl_typesupport_introspection_cpp") + +install( + TARGETS rmw_zenoh_cpp + EXPORT export_rmw_zenoh_cpp + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_package() diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml new file mode 100644 index 00000000..ab603469 --- /dev/null +++ b/rmw_zenoh_cpp/package.xml @@ -0,0 +1,21 @@ + + + + rmw_zenoh_cpp + 0.0.1 + A ROS 2 middleware implementation using zenoh-c + Yadunund + Apache License 2.0 + + ament_cmake + + zenoh_c_vendor + + rcpputils + rcutils + rmw + + + ament_cmake + + diff --git a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp new file mode 100644 index 00000000..8fa2a1f8 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp @@ -0,0 +1,25 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 SRC__DETAIL__RMW_INIT_OPTIONS_IMPL_HPP +#define SRC__DETAIL__RMW_INIT_OPTIONS_IMPL_HPP + +#include "zenoh.h" + +struct rmw_init_options_impl_t +{ + z_owned_config_t* config; +}; + +#endif // SRC__DETAIL__RMW_INIT_OPTIONS_IMPL_HPP diff --git a/rmw_zenoh_cpp/src/rmw_init_options.cpp b/rmw_zenoh_cpp/src/rmw_init_options.cpp new file mode 100644 index 00000000..598eba14 --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_init_options.cpp @@ -0,0 +1,15 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 "detail/rmw_init_options_impl.hpp" diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt new file mode 100644 index 00000000..4f937a57 --- /dev/null +++ b/zenoh_c_vendor/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.16) +project(zenoh_c_vendor) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_vendor_package REQUIRED) + +ament_vendor(zenoh_c_vendor + VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git + VCS_VERSION 0.10.0-dev +) + +# set(INSTALL_DIR "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-prefix/install") +# install( +# DIRECTORY "${INSTALL_DIR}/lib/" +# DESTINATION "lib/${PROJECT_NAME}" +# USE_SOURCE_PERMISSIONS +# ) + +ament_package() diff --git a/zenoh_c_vendor/package.xml b/zenoh_c_vendor/package.xml new file mode 100644 index 00000000..6515c1e9 --- /dev/null +++ b/zenoh_c_vendor/package.xml @@ -0,0 +1,21 @@ + + + + zenoh_c_vendor + 0.0.1 + Vendor pkg to install zenoh-c + Yadunund + Apache License 2.0 + + ament_cmake + + ament_cmake + ament_cmake_vendor_package + + + clang + + + ament_cmake + + From bb3617e65b5ca35e1253e98d6d9640e5965a31ae Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 5 Sep 2023 20:57:10 +0800 Subject: [PATCH 03/50] Add rmw functions Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 5 +- rmw_zenoh_cpp/package.xml | 1 + rmw_zenoh_cpp/src/detail/identifier.hpp | 20 + .../src/detail/rmw_init_options_impl.hpp | 6 + .../src/detail/serialization_format.hpp | 20 + rmw_zenoh_cpp/src/rmw_init_options.cpp | 34 + rmw_zenoh_cpp/src/rmw_zenoh.cpp | 742 ++++++++++++++++++ 7 files changed, 827 insertions(+), 1 deletion(-) create mode 100644 rmw_zenoh_cpp/src/detail/identifier.hpp create mode 100644 rmw_zenoh_cpp/src/detail/serialization_format.hpp create mode 100644 rmw_zenoh_cpp/src/rmw_zenoh.cpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 1f2f9bf0..e2555bd3 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -14,6 +14,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(fastcdr CONFIG REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) @@ -21,12 +22,14 @@ find_package(zenoh_c_vendor REQUIRED) find_package(zenohc REQUIRED) -add_library(rmw_zenoh_cpp +add_library(rmw_zenoh_cpp SHARED src/rmw_init_options.cpp + src/rmw_zenoh.cpp ) target_link_libraries(rmw_zenoh_cpp PRIVATE + rmw::rmw zenohc::lib ) diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index ab603469..c0483935 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -11,6 +11,7 @@ zenoh_c_vendor + fastcdr rcpputils rcutils rmw diff --git a/rmw_zenoh_cpp/src/detail/identifier.hpp b/rmw_zenoh_cpp/src/detail/identifier.hpp new file mode 100644 index 00000000..1c45ea0d --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/identifier.hpp @@ -0,0 +1,20 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 SRC__DETAIL__IDENTIFIER_HPP +#define SRC__DETAIL__IDENTIFIER_HPP + +extern const char * const rmw_zenoh_identifier = "rmw_zenoh_cpp"; + +#endif // SRC__DETAIL__IDENTIFIER_HPP diff --git a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp index 8fa2a1f8..5913332d 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp @@ -17,9 +17,15 @@ #include "zenoh.h" +namespace rmw_zenoh_cpp{ +namespace detail { + struct rmw_init_options_impl_t { z_owned_config_t* config; }; +} // namespace detail +} // namespace rmw_zenoh_cpp + #endif // SRC__DETAIL__RMW_INIT_OPTIONS_IMPL_HPP diff --git a/rmw_zenoh_cpp/src/detail/serialization_format.hpp b/rmw_zenoh_cpp/src/detail/serialization_format.hpp new file mode 100644 index 00000000..19b2c577 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/serialization_format.hpp @@ -0,0 +1,20 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 SRC__DETAIL__SERIALIZATION_FORMAT_HPP +#define SRC__DETAIL__SERIALIZATION_FORMAT_HPP + +extern const char * const rmw_zenoh_serialization_format = "cdr"; + +#endif // SRC__DETAIL__SERIALIZATION_FORMAT_HPP diff --git a/rmw_zenoh_cpp/src/rmw_init_options.cpp b/rmw_zenoh_cpp/src/rmw_init_options.cpp index 598eba14..3b46a0a6 100644 --- a/rmw_zenoh_cpp/src/rmw_init_options.cpp +++ b/rmw_zenoh_cpp/src/rmw_init_options.cpp @@ -13,3 +13,37 @@ // limitations under the License. #include "detail/rmw_init_options_impl.hpp" + +#include "rmw/rmw.h" + +//============================================================================== +/// Return a zero initialized init options structure. +// rmw_init_options_t +// rmw_get_zero_initialized_init_options(void) +// { +// return RMW_RET_UNSUPPORTED; +// } + +//============================================================================== +/// Initialize given init options with the default values and implementation specific values. +rmw_ret_t +rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t allocator) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Copy the given source init options to the destination init options. +rmw_ret_t +rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Finalize the given init options. +rmw_ret_t +rmw_init_options_fini(rmw_init_options_t * init_options) +{ + return RMW_RET_UNSUPPORTED; +} diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp new file mode 100644 index 00000000..f4558b74 --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -0,0 +1,742 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 "detail/identifier.hpp" +#include "detail/serialization_format.hpp" + +#include "rmw/rmw.h" + + +//============================================================================== +/// Get the name of the rmw implementation being used +const char * +rmw_get_implementation_identifier(void) +{ + return rmw_zenoh_identifier; +} + +//============================================================================== +/// Get the unique serialization format for this middleware. +const char * +rmw_get_serialization_format(void) +{ + return rmw_zenoh_serialization_format; +} + +//============================================================================== +/// Create a node and return a handle to that node. +rmw_node_t * +rmw_create_node( + rmw_context_t * context, + const char * name, + const char * namespace_) +{ + return nullptr; +} + +//============================================================================== +/// Finalize a given node handle, reclaim the resources, and deallocate the node handle. +rmw_ret_t +rmw_destroy_node(rmw_node_t * node) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Return a guard condition which is triggered when the ROS graph changes. +const rmw_guard_condition_t * +rmw_node_get_graph_guard_condition(const rmw_node_t * node) +{ + return nullptr; +} + +//============================================================================== +/// Initialize a publisher allocation to be used with later publications. +rmw_ret_t +rmw_init_publisher_allocation( + const rosidl_message_type_support_t * type_support, + const rosidl_runtime_c__Sequence__bound * message_bounds, + rmw_publisher_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Destroy a publisher allocation object. +rmw_ret_t +rmw_fini_publisher_allocation( + rmw_publisher_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Create a publisher and return a handle to that publisher. +rmw_publisher_t * +rmw_create_publisher( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rmw_qos_profile_t * qos_profile, + const rmw_publisher_options_t * publisher_options) +{ + return nullptr; +} + +//============================================================================== +/// Finalize a given publisher handle, reclaim the resources, and deallocate the publisher handle. +rmw_ret_t +rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Borrow a loaned ROS message. +rmw_ret_t +rmw_borrow_loaned_message( + const rmw_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + void ** ros_message) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Return a loaned message previously borrowed from a publisher. +rmw_ret_t +rmw_return_loaned_message_from_publisher( + const rmw_publisher_t * publisher, + void * loaned_message) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Publish a ROS message. +rmw_ret_t +rmw_publish( + const rmw_publisher_t * publisher, + const void * ros_message, + rmw_publisher_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Publish a loaned ROS message. +rmw_ret_t +rmw_publish_loaned_message( + const rmw_publisher_t * publisher, + void * ros_message, + rmw_publisher_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Retrieve the number of matched subscriptions to a publisher. +rmw_ret_t +rmw_publisher_count_matched_subscriptions( + const rmw_publisher_t * publisher, + size_t * subscription_count) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Retrieve the actual qos settings of the publisher. +rmw_ret_t +rmw_publisher_get_actual_qos( + const rmw_publisher_t * publisher, + rmw_qos_profile_t * qos) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Publish a ROS message as a byte stream. +rmw_ret_t +rmw_publish_serialized_message( + const rmw_publisher_t * publisher, + const rmw_serialized_message_t * serialized_message, + rmw_publisher_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Compute the size of a serialized message. +rmw_ret_t +rmw_get_serialized_message_size( + const rosidl_message_type_support_t * type_support, + const rosidl_runtime_c__Sequence__bound * message_bounds, + size_t * size) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) +rmw_ret_t +rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Wait until all published message data is acknowledged or until the specified timeout elapses. +rmw_ret_t +rmw_publisher_wait_for_all_acked( + const rmw_publisher_t * publisher, + rmw_time_t wait_timeout) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Serialize a ROS message into a rmw_serialized_message_t. +rmw_ret_t +rmw_serialize( + const void * ros_message, + const rosidl_message_type_support_t * type_support, + rmw_serialized_message_t * serialized_message) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Deserialize a ROS message. +rmw_ret_t +rmw_deserialize( + const rmw_serialized_message_t * serialized_message, + const rosidl_message_type_support_t * type_support, + void * ros_message) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Initialize a subscription allocation to be used with later `take`s. +rmw_ret_t +rmw_init_subscription_allocation( + const rosidl_message_type_support_t * type_support, + const rosidl_runtime_c__Sequence__bound * message_bounds, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Destroy a publisher allocation object. +rmw_ret_t +rmw_fini_subscription_allocation( + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Create a subscription and return a handle to that subscription. +rmw_subscription_t * +rmw_create_subscription( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options) +{ + return nullptr; +} + +//============================================================================== +/// Finalize a given subscription handle, reclaim the resources, and deallocate the subscription +rmw_ret_t +rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Retrieve the number of matched publishers to a subscription. +rmw_ret_t +rmw_subscription_count_matched_publishers( + const rmw_subscription_t * subscription, + size_t * publisher_count) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Retrieve the actual qos settings of the subscription. +rmw_ret_t +rmw_subscription_get_actual_qos( + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Set the content filter options for the subscription. +rmw_ret_t +rmw_subscription_set_content_filter( + rmw_subscription_t * subscription, + const rmw_subscription_content_filter_options_t * options) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Retrieve the content filter options of the subscription. +rmw_ret_t +rmw_subscription_get_content_filter( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_subscription_content_filter_options_t * options) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Take an incoming ROS message. +rmw_ret_t +rmw_take( + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Take an incoming ROS message with its metadata. +rmw_ret_t +rmw_take_with_info( + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Take multiple incoming ROS messages with their metadata. +rmw_ret_t +rmw_take_sequence( + const rmw_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t * taken, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Take an incoming ROS message as a byte stream. +rmw_ret_t +rmw_take_serialized_message( + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Take an incoming ROS message as a byte stream with its metadata. +rmw_ret_t +rmw_take_serialized_message_with_info( + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Take an incoming ROS message, loaned by the middleware. +rmw_ret_t +rmw_take_loaned_message( + const rmw_subscription_t * subscription, + void ** loaned_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Take a loaned message and with its additional message information. +rmw_ret_t +rmw_take_loaned_message_with_info( + const rmw_subscription_t * subscription, + void ** loaned_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Return a loaned ROS message previously taken from a subscription. +rmw_ret_t +rmw_return_loaned_message_from_subscription( + const rmw_subscription_t * subscription, + void * loaned_message) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Create a service client that can send requests to and receive replies from a service server. +rmw_client_t * +rmw_create_client( + const rmw_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rmw_qos_profile_t * qos_policies) +{ + return nullptr; +} + +//============================================================================== +/// Destroy and unregister a service client from its node. +rmw_ret_t +rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Send a ROS service request. +rmw_ret_t +rmw_send_request( + const rmw_client_t * client, + const void * ros_request, + int64_t * sequence_id) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Take an incoming ROS service response. +rmw_ret_t +rmw_take_response( + const rmw_client_t * client, + rmw_service_info_t * request_header, + void * ros_response, + bool * taken) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Retrieve the actual qos settings of the client's request publisher. +rmw_ret_t +rmw_client_request_publisher_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Retrieve the actual qos settings of the client's response subscription. +rmw_ret_t +rmw_client_response_subscription_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Create a service server that can receive requests from and send replies to a service client. +rmw_service_t * +rmw_create_service( + const rmw_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rmw_qos_profile_t * qos_profile) +{ + return nullptr; +} + +//============================================================================== +/// Destroy and unregister a service server from its node. +rmw_ret_t +rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Take an incoming ROS service request. +rmw_ret_t +rmw_take_request( + const rmw_service_t * service, + rmw_service_info_t * request_header, + void * ros_request, + bool * taken) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Send a ROS service response. +rmw_ret_t +rmw_send_response( + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_response) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Retrieve the actual qos settings of the service's request subscription. +rmw_ret_t +rmw_service_request_subscription_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Retrieve the actual qos settings of the service's response publisher. +rmw_ret_t +rmw_service_response_publisher_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Create a guard condition and return a handle to that guard condition. +rmw_guard_condition_t * +rmw_create_guard_condition(rmw_context_t * context) +{ + return nullptr; +} + +/// Finalize a given guard condition handle, reclaim the resources, and deallocate the handle. +rmw_ret_t +rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +rmw_ret_t +rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Create a wait set to store conditions that the middleware can wait on. +rmw_wait_set_t * +rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) +{ + return nullptr; +} + +//============================================================================== +/// Destroy a wait set. +rmw_ret_t +rmw_destroy_wait_set(rmw_wait_set_t * wait_set) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Waits on sets of different entities and returns when one is ready. +rmw_ret_t +rmw_wait( + rmw_subscriptions_t * subscriptions, + rmw_guard_conditions_t * guard_conditions, + rmw_services_t * services, + rmw_clients_t * clients, + rmw_events_t * events, + rmw_wait_set_t * wait_set, + const rmw_time_t * wait_timeout) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Return the name and namespace of all nodes in the ROS graph. +rmw_ret_t +rmw_get_node_names( + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Return the name, namespae, and enclave name of all nodes in the ROS graph. +rmw_ret_t +rmw_get_node_names_with_enclaves( + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * enclaves) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Count the number of known publishers matching a topic name. +rmw_ret_t +rmw_count_publishers( + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Count the number of known subscribers matching a topic name. +rmw_ret_t +rmw_count_subscribers( + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Count the number of known clients matching a service name. +rmw_ret_t +rmw_count_clients( + const rmw_node_t * node, + const char * service_name, + size_t * count) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Count the number of known servers matching a service name. +rmw_ret_t +rmw_count_services( + const rmw_node_t * node, + const char * service_name, + size_t * count) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Get the globally unique identifier (GID) of a publisher. +rmw_ret_t +rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Get the globally unique identifier (GID) of a service client. +rmw_ret_t +rmw_get_gid_for_client(const rmw_client_t * client, rmw_gid_t * gid) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Check if two globally unique identifiers (GIDs) are equal. +rmw_ret_t +rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * result) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Check if a service server is available for the given service client. +rmw_ret_t +rmw_service_server_is_available( + const rmw_node_t * node, + const rmw_client_t * client, + bool * is_available) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Set the current log severity +rmw_ret_t +rmw_set_log_severity(rmw_log_severity_t severity) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Set the on new message callback function for the subscription. +rmw_ret_t +rmw_subscription_set_on_new_message_callback( + rmw_subscription_t * subscription, + rmw_event_callback_t callback, + const void * user_data) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Set the on new request callback function for the service. +rmw_ret_t +rmw_service_set_on_new_request_callback( + rmw_service_t * service, + rmw_event_callback_t callback, + const void * user_data) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Set the on new response callback function for the client. +rmw_ret_t +rmw_client_set_on_new_response_callback( + rmw_client_t * client, + rmw_event_callback_t callback, + const void * user_data) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +/// Set the callback function for the event. +rmw_ret_t +rmw_event_set_callback( + rmw_event_t * event, + rmw_event_callback_t callback, + const void * user_data) +{ + return RMW_RET_UNSUPPORTED; +} From 0cadf256effeacf7e009f8fda0bbe2c4e37db96b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 5 Sep 2023 20:57:55 +0800 Subject: [PATCH 04/50] Uncrustify Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp index 5913332d..38722ed0 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp @@ -17,12 +17,14 @@ #include "zenoh.h" -namespace rmw_zenoh_cpp{ -namespace detail { +namespace rmw_zenoh_cpp +{ +namespace detail +{ struct rmw_init_options_impl_t { - z_owned_config_t* config; + z_owned_config_t * config; }; } // namespace detail From 53a1ea42e61c3d9070b84daa82e0f1ce981ab8fa Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 5 Sep 2023 21:16:14 +0800 Subject: [PATCH 05/50] Add other endpoint related functions Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 4 ++ .../src/rmw_get_node_info_and_types.cpp | 70 +++++++++++++++++++ .../src/rmw_get_service_names_and_types.cpp | 27 +++++++ .../src/rmw_get_topic_endpoint_info.cpp | 42 +++++++++++ .../src/rmw_get_topic_names_and_types.cpp | 28 ++++++++ 5 files changed, 171 insertions(+) create mode 100644 rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp create mode 100644 rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp create mode 100644 rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp create mode 100644 rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index e2555bd3..b1627128 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -23,6 +23,10 @@ find_package(zenohc REQUIRED) add_library(rmw_zenoh_cpp SHARED + src/rmw_get_node_info_and_types.cpp + src/rmw_get_service_names_and_types.cpp + src/rmw_get_topic_endpoint_info.cpp + src/rmw_get_topic_names_and_types.cpp src/rmw_init_options.cpp src/rmw_zenoh.cpp ) diff --git a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp new file mode 100644 index 00000000..019b069e --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp @@ -0,0 +1,70 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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_node_info_and_types.h" + +///============================================================================== +// Return all topic names and types for which a given remote node has subscriptions. +rmw_ret_t +rmw_get_subscriber_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + return RMW_RET_UNSUPPORTED; +} + +///============================================================================== +/// Return all topic names and types for which a given remote node has publishers. +rmw_ret_t +rmw_get_publisher_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + return RMW_RET_UNSUPPORTED; +} + +///============================================================================== +/// Return all service names and types for which a given remote node has servers. +rmw_ret_t +rmw_get_service_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rmw_names_and_types_t * service_names_and_types) +{ + return RMW_RET_UNSUPPORTED; +} + +///============================================================================== +/// Return all service names and types for which a given remote node has clients. +rmw_ret_t +rmw_get_client_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rmw_names_and_types_t * service_names_and_types) +{ + return RMW_RET_UNSUPPORTED; +} diff --git a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp new file mode 100644 index 00000000..4dacdb21 --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp @@ -0,0 +1,27 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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_service_names_and_types.h" + +///============================================================================== +/// Return all service names and types in the ROS graph. +rmw_ret_t +rmw_get_service_names_and_types( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * service_names_and_types) +{ + return RMW_RET_UNSUPPORTED; +} diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp new file mode 100644 index 00000000..cc3b1247 --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -0,0 +1,42 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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_topic_endpoint_info.h" + +///============================================================================== +/// Retrieve endpoint information for each known publisher of a given topic. +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) +{ + return RMW_RET_UNSUPPORTED; +} + +///============================================================================== +/// Retrieve endpoint information for each known subscription of a given topic. +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) +{ + return RMW_RET_UNSUPPORTED; +} diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp new file mode 100644 index 00000000..901485fc --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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_topic_names_and_types.h" + +///============================================================================== +/// Return all topic names and types in the ROS graph. +rmw_ret_t +rmw_get_topic_names_and_types( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + return RMW_RET_UNSUPPORTED; +} From c2af822dc56f0e5ba8d86873584cb98359be4855 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 5 Sep 2023 21:24:01 +0800 Subject: [PATCH 06/50] Wrap code in extern c block Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp | 4 ++++ rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp | 4 ++++ rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp | 4 ++++ rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp | 4 ++++ rmw_zenoh_cpp/src/rmw_init_options.cpp | 4 ++++ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 5 ++++- 6 files changed, 24 insertions(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp index 019b069e..55d417c5 100644 --- a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp @@ -15,6 +15,8 @@ #include "rmw/get_node_info_and_types.h" +extern "C" +{ ///============================================================================== // Return all topic names and types for which a given remote node has subscriptions. rmw_ret_t @@ -68,3 +70,5 @@ rmw_get_client_names_and_types_by_node( { return RMW_RET_UNSUPPORTED; } + +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp index 4dacdb21..e16f86b2 100644 --- a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp @@ -15,6 +15,8 @@ #include "rmw/get_service_names_and_types.h" +extern "C" +{ ///============================================================================== /// Return all service names and types in the ROS graph. rmw_ret_t @@ -25,3 +27,5 @@ rmw_get_service_names_and_types( { return RMW_RET_UNSUPPORTED; } + +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp index cc3b1247..aa1169b4 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -15,6 +15,8 @@ #include "rmw/get_topic_endpoint_info.h" +extern "C" +{ ///============================================================================== /// Retrieve endpoint information for each known publisher of a given topic. rmw_ret_t @@ -40,3 +42,5 @@ rmw_get_subscriptions_info_by_topic( { return RMW_RET_UNSUPPORTED; } + +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp index 901485fc..d5408e23 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp @@ -15,6 +15,8 @@ #include "rmw/get_topic_names_and_types.h" +extern "C" +{ ///============================================================================== /// Return all topic names and types in the ROS graph. rmw_ret_t @@ -26,3 +28,5 @@ rmw_get_topic_names_and_types( { return RMW_RET_UNSUPPORTED; } + +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_init_options.cpp b/rmw_zenoh_cpp/src/rmw_init_options.cpp index 3b46a0a6..9de77477 100644 --- a/rmw_zenoh_cpp/src/rmw_init_options.cpp +++ b/rmw_zenoh_cpp/src/rmw_init_options.cpp @@ -16,6 +16,8 @@ #include "rmw/rmw.h" +extern "C" +{ //============================================================================== /// Return a zero initialized init options structure. // rmw_init_options_t @@ -47,3 +49,5 @@ rmw_init_options_fini(rmw_init_options_t * init_options) { return RMW_RET_UNSUPPORTED; } + +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index f4558b74..5bbe920d 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -17,7 +17,8 @@ #include "rmw/rmw.h" - +extern "C" +{ //============================================================================== /// Get the name of the rmw implementation being used const char * @@ -740,3 +741,5 @@ rmw_event_set_callback( { return RMW_RET_UNSUPPORTED; } + +} // extern "C" From b582e7ea720de9e2c3ba9c4f752ccf294d8d0bcb Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 6 Sep 2023 00:50:23 +0800 Subject: [PATCH 07/50] Added impl for init_options Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 1 + rmw_zenoh_cpp/src/detail/identifier.hpp | 2 +- .../src/detail/rmw_init_options_impl.hpp | 14 +-- .../src/detail/serialization_format.hpp | 2 +- rmw_zenoh_cpp/src/rmw_init_options.cpp | 105 ++++++++++++++++-- 5 files changed, 102 insertions(+), 22 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index b1627128..396a1551 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(rmw_zenoh_cpp SHARED target_link_libraries(rmw_zenoh_cpp PRIVATE + rcutils::rcutils rmw::rmw zenohc::lib ) diff --git a/rmw_zenoh_cpp/src/detail/identifier.hpp b/rmw_zenoh_cpp/src/detail/identifier.hpp index 1c45ea0d..cb850960 100644 --- a/rmw_zenoh_cpp/src/detail/identifier.hpp +++ b/rmw_zenoh_cpp/src/detail/identifier.hpp @@ -15,6 +15,6 @@ #ifndef SRC__DETAIL__IDENTIFIER_HPP #define SRC__DETAIL__IDENTIFIER_HPP -extern const char * const rmw_zenoh_identifier = "rmw_zenoh_cpp"; +extern inline const char * const rmw_zenoh_identifier = "rmw_zenoh_cpp"; #endif // SRC__DETAIL__IDENTIFIER_HPP diff --git a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp index 38722ed0..b6bf7474 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp @@ -17,17 +17,13 @@ #include "zenoh.h" -namespace rmw_zenoh_cpp -{ -namespace detail +struct rmw_init_options_impl_s { + // A loaned config. + z_config_t config; -struct rmw_init_options_impl_t -{ - z_owned_config_t * config; + // And owned session. + z_owned_session_t session; }; -} // namespace detail -} // namespace rmw_zenoh_cpp - #endif // SRC__DETAIL__RMW_INIT_OPTIONS_IMPL_HPP diff --git a/rmw_zenoh_cpp/src/detail/serialization_format.hpp b/rmw_zenoh_cpp/src/detail/serialization_format.hpp index 19b2c577..2c4a3b96 100644 --- a/rmw_zenoh_cpp/src/detail/serialization_format.hpp +++ b/rmw_zenoh_cpp/src/detail/serialization_format.hpp @@ -15,6 +15,6 @@ #ifndef SRC__DETAIL__SERIALIZATION_FORMAT_HPP #define SRC__DETAIL__SERIALIZATION_FORMAT_HPP -extern const char * const rmw_zenoh_serialization_format = "cdr"; +extern inline const char * const rmw_zenoh_serialization_format = "cdr"; #endif // SRC__DETAIL__SERIALIZATION_FORMAT_HPP diff --git a/rmw_zenoh_cpp/src/rmw_init_options.cpp b/rmw_zenoh_cpp/src/rmw_init_options.cpp index 9de77477..a07eccba 100644 --- a/rmw_zenoh_cpp/src/rmw_init_options.cpp +++ b/rmw_zenoh_cpp/src/rmw_init_options.cpp @@ -12,26 +12,78 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "detail/identifier.hpp" #include "detail/rmw_init_options_impl.hpp" -#include "rmw/rmw.h" + +#include "rcutils/env.h" +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/init.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/init_options.h" + +#include "zenoh.h" extern "C" { -//============================================================================== -/// Return a zero initialized init options structure. -// rmw_init_options_t -// rmw_get_zero_initialized_init_options(void) -// { -// return RMW_RET_UNSUPPORTED; -// } - //============================================================================== /// Initialize given init options with the default values and implementation specific values. rmw_ret_t rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t allocator) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + if (NULL != init_options->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + + init_options->instance_id = 0; + init_options->implementation_identifier = rmw_zenoh_identifier; + init_options->allocator = allocator; + init_options->impl = nullptr; + init_options->enclave = NULL; + init_options->domain_id = RMW_DEFAULT_DOMAIN_ID; + init_options->security_options = rmw_get_default_security_options(); + init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT; + init_options->discovery_options = rmw_get_zero_initialized_discovery_options(); + return rmw_discovery_options_init(&(init_options->discovery_options), 0, &allocator); + + // Initialize impl. + init_options->impl = static_cast( + allocator.allocate(sizeof(rmw_init_options_impl_s), allocator.state)); + if (!init_options->impl) { + RMW_SET_ERROR_MSG("failed to allocate init_options impl"); + allocator.deallocate(init_options->enclave, allocator.state); + return RMW_RET_BAD_ALLOC; + } + + // Initialize the zenoh config. We use the default config if a path to the + // config is unavailable. + z_owned_config_t config; + const char * zenoh_config_path; + if (nullptr != rcutils_get_env("ZENOH_CONFIG_PATH", &zenoh_config_path)) { + // No config path set. + config = z_config_default(); + } else { + config = zc_config_from_file(zenoh_config_path); + if (!z_config_check(&config)) { + RMW_SET_ERROR_MSG("Error in zenoh config path"); + return RMW_RET_INVALID_ARGUMENT; + } + } + + // Initialize the zenoh session. + init_options->impl->config = z_config_loan(&config); + init_options->impl->session = z_open(z_move(config)); + if (!z_session_check(&init_options->impl->session)) { + RMW_SET_ERROR_MSG("Error setting up zenoh session"); + return RMW_RET_INVALID_ARGUMENT; + } + + return RMW_RET_OK; } //============================================================================== @@ -47,7 +99,38 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + if (NULL == init_options->implementation_identifier) { + RMW_SET_ERROR_MSG("expected initialized init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init_options, + init_options->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + rcutils_allocator_t * allocator = &init_options->allocator; + RCUTILS_CHECK_ALLOCATOR(allocator, return RMW_RET_INVALID_ARGUMENT); + + allocator->deallocate(init_options->enclave, allocator->state); + rmw_ret_t ret = rmw_security_options_fini(&init_options->security_options, allocator); + if (ret != RMW_RET_OK) { + return ret; + } + + // Close the zenoh session and deallocate the impl. + rmw_init_options_impl_s * impl = static_cast(init_options->impl); + if (nullptr == impl) { + RMW_SET_ERROR_MSG("invalid impl in init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + z_close(z_move(impl->session)); + allocator->deallocate(init_options->impl, allocator->state); + + ret = rmw_discovery_options_fini(&init_options->discovery_options); + *init_options = rmw_get_zero_initialized_init_options(); + + return ret; } } // extern "C" From b9da7ab84ffa1bd003a5d18a68873b711f0397ed Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 6 Sep 2023 06:39:11 +0800 Subject: [PATCH 08/50] Implement init functions Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 2 + rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp | 30 ++++ .../src/detail/rmw_init_options_impl.hpp | 8 +- rmw_zenoh_cpp/src/rmw_init.cpp | 158 ++++++++++++++++++ rmw_zenoh_cpp/src/rmw_init_options.cpp | 82 ++++----- 5 files changed, 229 insertions(+), 51 deletions(-) create mode 100644 rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp create mode 100644 rmw_zenoh_cpp/src/rmw_init.cpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 396a1551..35cab747 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -28,11 +28,13 @@ add_library(rmw_zenoh_cpp SHARED src/rmw_get_topic_endpoint_info.cpp src/rmw_get_topic_names_and_types.cpp src/rmw_init_options.cpp + src/rmw_init.cpp src/rmw_zenoh.cpp ) target_link_libraries(rmw_zenoh_cpp PRIVATE + rcpputils::rcpputils rcutils::rcutils rmw::rmw zenohc::lib diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp new file mode 100644 index 00000000..f5fd2eaf --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp @@ -0,0 +1,30 @@ + +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 SRC__DETAIL__RMW_CONTEXT_IMPL_HPP +#define SRC__DETAIL__RMW_CONTEXT_IMPL_HPP + +#include "zenoh.h" + +struct rmw_context_impl_s +{ + // An owned session. + z_owned_session_t session; + + /// Shutdown flag. + bool is_shutdown; +}; + +#endif // SRC__DETAIL__RMW_CONTEXT_IMPL_HPP diff --git a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp index b6bf7474..8d0edc8c 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp @@ -17,13 +17,11 @@ #include "zenoh.h" +// TODO(YV): Consider using this again. struct rmw_init_options_impl_s { - // A loaned config. - z_config_t config; - - // And owned session. - z_owned_session_t session; + // An owned config. + z_owned_config_t config; }; #endif // SRC__DETAIL__RMW_INIT_OPTIONS_IMPL_HPP diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp new file mode 100644 index 00000000..1c210431 --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -0,0 +1,158 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 "detail/identifier.hpp" +#include "detail/rmw_context_impl.hpp" + + +#include "rcutils/env.h" +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/init.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/init_options.h" + +#include "rcpputils/scope_exit.hpp" + +#include "zenoh.h" + +extern "C" +{ +//============================================================================== +/// Initialize the middleware with the given options, and yielding an context. +rmw_ret_t +rmw_init(const rmw_init_options_t * options, rmw_context_t * context) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + options->implementation_identifier, + "expected initialized init options", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + options, + options->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_FOR_NULL_WITH_MSG( + options->enclave, + "expected non-null enclave", + return RMW_RET_INVALID_ARGUMENT); + if (NULL != context->implementation_identifier) { + RMW_SET_ERROR_MSG("expected a zero-initialized context"); + return RMW_RET_INVALID_ARGUMENT; + } + + auto restore_context = rcpputils::make_scope_exit( + [context]() {*context = rmw_get_zero_initialized_context();}); + + context->instance_id = options->instance_id; + context->implementation_identifier = rmw_zenoh_identifier; + // No custom handling of RMW_DEFAULT_DOMAIN_ID. Simply use a reasonable domain id. + context->actual_domain_id = + RMW_DEFAULT_DOMAIN_ID != options->domain_id ? options->domain_id : 0u; + + context->impl = new (std::nothrow) rmw_context_impl_t(); + if (nullptr == context->impl) { + RMW_SET_ERROR_MSG("failed to allocate context impl"); + return RMW_RET_BAD_ALLOC; + } + auto cleanup_impl = rcpputils::make_scope_exit( + [context]() {delete context->impl;}); + + rmw_ret_t ret; + if ((ret = rmw_init_options_copy(options, &context->options)) != RMW_RET_OK) { + return ret; + } + + context->impl->is_shutdown = false; + + // Initialize the zenoh config. We use the default config if a path to the + // config is unavailable. + z_owned_config_t config; + const char * zenoh_config_path; + if (nullptr != rcutils_get_env("ZENOH_CONFIG_PATH", &zenoh_config_path)) { + // No config path set. + config = z_config_default(); + } else { + config = zc_config_from_file(zenoh_config_path); + if (!z_config_check(&config)) { + RMW_SET_ERROR_MSG("Error in zenoh config path"); + return RMW_RET_INVALID_ARGUMENT; + } + } + // Initialize the zenoh session. + context->impl->session = z_open(z_move(config)); + if (!z_session_check(&context->impl->session)) { + RMW_SET_ERROR_MSG("Error setting up zenoh session"); + return RMW_RET_INVALID_ARGUMENT; + } + + cleanup_impl.cancel(); + restore_context.cancel(); + return RMW_RET_OK; +} + +//============================================================================== +/// Shutdown the middleware for a given context. +rmw_ret_t +rmw_shutdown(rmw_context_t * context) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + // Close the zenoh session + if (z_close(z_move(context->impl->session)) < 0) + { + RMW_SET_ERROR_MSG("Error while closing zenoh session"); + return RMW_RET_ERROR; + } + context->impl->is_shutdown = true; + return RMW_RET_OK; +} + +//============================================================================== +/// Finalize a context. +rmw_ret_t +rmw_context_fini(rmw_context_t * context) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!context->impl->is_shutdown) { + RCUTILS_SET_ERROR_MSG("context has not been shutdown"); + return RMW_RET_INVALID_ARGUMENT; + } + rmw_ret_t ret = rmw_init_options_fini(&context->options); + delete context->impl; + *context = rmw_get_zero_initialized_context(); + return ret; +} +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_init_options.cpp b/rmw_zenoh_cpp/src/rmw_init_options.cpp index a07eccba..ffbadfbd 100644 --- a/rmw_zenoh_cpp/src/rmw_init_options.cpp +++ b/rmw_zenoh_cpp/src/rmw_init_options.cpp @@ -16,11 +16,9 @@ #include "detail/rmw_init_options_impl.hpp" -#include "rcutils/env.h" #include "rcutils/strdup.h" #include "rcutils/types.h" -#include "rmw/init.h" #include "rmw/impl/cpp/macros.hpp" #include "rmw/init_options.h" @@ -50,40 +48,6 @@ rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t all init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT; init_options->discovery_options = rmw_get_zero_initialized_discovery_options(); return rmw_discovery_options_init(&(init_options->discovery_options), 0, &allocator); - - // Initialize impl. - init_options->impl = static_cast( - allocator.allocate(sizeof(rmw_init_options_impl_s), allocator.state)); - if (!init_options->impl) { - RMW_SET_ERROR_MSG("failed to allocate init_options impl"); - allocator.deallocate(init_options->enclave, allocator.state); - return RMW_RET_BAD_ALLOC; - } - - // Initialize the zenoh config. We use the default config if a path to the - // config is unavailable. - z_owned_config_t config; - const char * zenoh_config_path; - if (nullptr != rcutils_get_env("ZENOH_CONFIG_PATH", &zenoh_config_path)) { - // No config path set. - config = z_config_default(); - } else { - config = zc_config_from_file(zenoh_config_path); - if (!z_config_check(&config)) { - RMW_SET_ERROR_MSG("Error in zenoh config path"); - return RMW_RET_INVALID_ARGUMENT; - } - } - - // Initialize the zenoh session. - init_options->impl->config = z_config_loan(&config); - init_options->impl->session = z_open(z_move(config)); - if (!z_session_check(&init_options->impl->session)) { - RMW_SET_ERROR_MSG("Error setting up zenoh session"); - return RMW_RET_INVALID_ARGUMENT; - } - - return RMW_RET_OK; } //============================================================================== @@ -91,7 +55,42 @@ rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t all rmw_ret_t rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); + if (NULL == src->implementation_identifier) { + RMW_SET_ERROR_MSG("expected initialized dst"); + return RMW_RET_INVALID_ARGUMENT; + } + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + src, + src->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (NULL != dst->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized dst"); + return RMW_RET_INVALID_ARGUMENT; + } + rcutils_allocator_t allocator = src->allocator; + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + rmw_init_options_t tmp = *src; + tmp.enclave = rcutils_strdup(tmp.enclave, allocator); + if (NULL != src->enclave && NULL == tmp.enclave) { + return RMW_RET_BAD_ALLOC; + } + tmp.security_options = rmw_get_zero_initialized_security_options(); + rmw_ret_t ret = + rmw_security_options_copy(&src->security_options, &allocator, &tmp.security_options); + if (RMW_RET_OK != ret) { + allocator.deallocate(tmp.enclave, allocator.state); + return ret; + } + tmp.discovery_options = rmw_get_zero_initialized_discovery_options(); + ret = rmw_discovery_options_copy( + &src->discovery_options, + &allocator, + &tmp.discovery_options); + *dst = tmp; + return RMW_RET_OK; } //============================================================================== @@ -118,15 +117,6 @@ rmw_init_options_fini(rmw_init_options_t * init_options) return ret; } - // Close the zenoh session and deallocate the impl. - rmw_init_options_impl_s * impl = static_cast(init_options->impl); - if (nullptr == impl) { - RMW_SET_ERROR_MSG("invalid impl in init_options"); - return RMW_RET_INVALID_ARGUMENT; - } - z_close(z_move(impl->session)); - allocator->deallocate(init_options->impl, allocator->state); - ret = rmw_discovery_options_fini(&init_options->discovery_options); *init_options = rmw_get_zero_initialized_init_options(); From 267ec444c34a2663beaa577c0e93c6e86ffa9663 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 6 Sep 2023 07:00:08 +0800 Subject: [PATCH 09/50] Fix config creation Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/rmw_init.cpp | 11 ++++++++--- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 1 + 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 1c210431..d29f1020 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -83,10 +83,15 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // config is unavailable. z_owned_config_t config; const char * zenoh_config_path; - if (nullptr != rcutils_get_env("ZENOH_CONFIG_PATH", &zenoh_config_path)) { - // No config path set. + if (NULL != rcutils_get_env("ZENOH_CONFIG_PATH", &zenoh_config_path)) { + RMW_SET_ERROR_MSG("Error in reading ZENOH_CONFIG_PATH envar"); + return RMW_RET_INVALID_ARGUMENT; + } + if (zenoh_config_path[0] == '\0'){ + // No config path set. config = z_config_default(); - } else { + } + else { config = zc_config_from_file(zenoh_config_path); if (!z_config_check(&config)) { RMW_SET_ERROR_MSG("Error in zenoh config path"); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 5bbe920d..7a396461 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "detail/identifier.hpp" +#include "detail/rmw_context_impl.hpp" #include "detail/serialization_format.hpp" #include "rmw/rmw.h" From dc0e22cf9ffc8465520664264285596c6ca228c9 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 7 Sep 2023 01:17:14 +0800 Subject: [PATCH 10/50] Impl create node Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 3 + rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp | 1 - rmw_zenoh_cpp/src/rmw_init.cpp | 14 ++-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 78 ++++++++++++++++++- 4 files changed, 86 insertions(+), 10 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 35cab747..4f89aaf6 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -11,6 +11,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +# TODO(yadunund): Remove this eventually. +add_compile_options(-Wno-unused-parameter) + # find dependencies find_package(ament_cmake REQUIRED) diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp index f5fd2eaf..869c7dab 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp @@ -1,4 +1,3 @@ - // Copyright 2023 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index d29f1020..43baa4df 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -84,14 +84,13 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) z_owned_config_t config; const char * zenoh_config_path; if (NULL != rcutils_get_env("ZENOH_CONFIG_PATH", &zenoh_config_path)) { - RMW_SET_ERROR_MSG("Error in reading ZENOH_CONFIG_PATH envar"); - return RMW_RET_INVALID_ARGUMENT; + RMW_SET_ERROR_MSG("Error in reading ZENOH_CONFIG_PATH envar"); + return RMW_RET_INVALID_ARGUMENT; } - if (zenoh_config_path[0] == '\0'){ - // No config path set. + if (zenoh_config_path[0] == '\0') { + // No config path set. config = z_config_default(); - } - else { + } else { config = zc_config_from_file(zenoh_config_path); if (!z_config_check(&config)) { RMW_SET_ERROR_MSG("Error in zenoh config path"); @@ -127,8 +126,7 @@ rmw_shutdown(rmw_context_t * context) return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); // Close the zenoh session - if (z_close(z_move(context->impl->session)) < 0) - { + if (z_close(z_move(context->impl->session)) < 0) { RMW_SET_ERROR_MSG("Error while closing zenoh session"); return RMW_RET_ERROR; } diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 7a396461..83acf239 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -16,7 +16,19 @@ #include "detail/rmw_context_impl.hpp" #include "detail/serialization_format.hpp" +#include "rcpputils/scope_exit.hpp" + +#include "rcutils/env.h" +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/ret_types.h" #include "rmw/rmw.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" extern "C" { @@ -44,7 +56,71 @@ rmw_create_node( const char * name, const char * namespace_) { - return nullptr; + RMW_CHECK_ARGUMENT_FOR_NULL(context, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + rmw_zenoh_identifier, + return nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return nullptr); + if (context->impl->is_shutdown) { + RCUTILS_SET_ERROR_MSG("context has been shutdown"); + return nullptr; + } + int validation_result = RMW_NODE_NAME_VALID; + rmw_ret_t ret = rmw_validate_node_name(name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_NODE_NAME_VALID != validation_result) { + const char * reason = rmw_node_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid node name: %s", reason); + return nullptr; + } + validation_result = RMW_NAMESPACE_VALID; + ret = rmw_validate_namespace(namespace_, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_NAMESPACE_VALID != validation_result) { + const char * reason = rmw_node_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid node namespace: %s", reason); + return nullptr; + } + + rmw_node_t * node = rmw_node_allocate(); + RMW_CHECK_FOR_NULL_WITH_MSG( + node, + "unable to allocate memory for rmw_node_t", + return nullptr); + auto cleanup_node = rcpputils::make_scope_exit( + [node]() { + rmw_free(const_cast(node->name)); + rmw_free(const_cast(node->namespace_)); + rmw_node_free(node); + }); + + node->name = static_cast(rmw_allocate(sizeof(char) * strlen(name) + 1)); + RMW_CHECK_ARGUMENT_FOR_NULL(node->name, nullptr); + memcpy(const_cast(node->name), name, strlen(name) + 1); + + node->namespace_ = + static_cast(rmw_allocate(sizeof(char) * strlen(namespace_) + 1)); + RMW_CHECK_ARGUMENT_FOR_NULL(node->namespace_, nullptr); + memcpy(const_cast(node->namespace_), namespace_, strlen(namespace_) + 1); + + // TODO(yadunund): Register with storage system here and throw error if + // zenohd is not running. + // Put metadata into node->data. + + cleanup_node.cancel(); + node->implementation_identifier = rmw_zenoh_identifier; + node->context = context; + return node; + } //============================================================================== From c41ddfade90cc89576c15b5cc36e7a32e8313cd3 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 7 Sep 2023 18:12:33 +0800 Subject: [PATCH 11/50] Return node guard condition Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp | 6 +++++ rmw_zenoh_cpp/src/rmw_init.cpp | 1 + rmw_zenoh_cpp/src/rmw_zenoh.cpp | 26 +++++++++++++++++-- 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp index 869c7dab..4f2d1d78 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp @@ -15,6 +15,8 @@ #ifndef SRC__DETAIL__RMW_CONTEXT_IMPL_HPP #define SRC__DETAIL__RMW_CONTEXT_IMPL_HPP +#include "rmw/rmw.h" + #include "zenoh.h" struct rmw_context_impl_s @@ -24,6 +26,10 @@ struct rmw_context_impl_s /// Shutdown flag. bool is_shutdown; + + // Equivalent to rmw_dds_common::Context's guard condition + /// Guard condition that should be triggered when the graph changes. + rmw_guard_condition_t * graph_guard_condition; }; #endif // SRC__DETAIL__RMW_CONTEXT_IMPL_HPP diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 43baa4df..ca285b0a 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -77,6 +77,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return ret; } + // Initialize context's implementation context->impl->is_shutdown = false; // Initialize the zenoh config. We use the default config if a path to the diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 83acf239..33ab4b2a 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -128,7 +128,22 @@ rmw_create_node( rmw_ret_t rmw_destroy_node(rmw_node_t * node) { - return RMW_RET_UNSUPPORTED; + rmw_ret_t result_ret = RMW_RET_OK; + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + // TODO(Yadunund): Unregister with storage system. + + // rmw_context_t * context = node->context; + rmw_free(const_cast(node->name)); + rmw_free(const_cast(node->namespace_)); + rmw_node_free(const_cast(node)); + // delete node_impl; + return result_ret; } //============================================================================== @@ -136,7 +151,14 @@ rmw_destroy_node(rmw_node_t * node) const rmw_guard_condition_t * rmw_node_get_graph_guard_condition(const rmw_node_t * node) { - return nullptr; + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + rmw_zenoh_identifier, + return nullptr); + // TODO(Yadunund): Also check if node->data is valud. + return node->context->impl->graph_guard_condition; } //============================================================================== From 3c0dc8d2b0ab23fa516f6c78c99907daeae716ee Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 7 Sep 2023 21:06:06 +0800 Subject: [PATCH 12/50] create publisher Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 30 ++++ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 166 +++++++++++++++++++- 2 files changed, 192 insertions(+), 4 deletions(-) create mode 100644 rmw_zenoh_cpp/src/detail/rmw_data_types.hpp diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp new file mode 100644 index 00000000..4f049a64 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -0,0 +1,30 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 SRC__DETAIL__RMW_DATA_TYPES_HPP +#define SRC__DETAIL__RMW_DATA_TYPES_HPP + +#include "rmw/rmw.h" + +#include "zenoh.h" + +/// Structs for various type erased data fields. +///============================================================================== +struct rmw_publisher_data_t +{ + // An owned publisher. + z_owned_publisher_t pub; +}; + +#endif // SRC__DETAIL__RMW_DATA_TYPES_HPP diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 33ab4b2a..b5c7e59f 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -14,6 +14,7 @@ #include "detail/identifier.hpp" #include "detail/rmw_context_impl.hpp" +#include "detail/rmw_data_types.hpp" #include "detail/serialization_format.hpp" #include "rcpputils/scope_exit.hpp" @@ -169,6 +170,10 @@ rmw_init_publisher_allocation( const rosidl_runtime_c__Sequence__bound * message_bounds, rmw_publisher_allocation_t * allocation) { + static_cast(type_support); + static_cast(message_bounds); + static_cast(allocation); + RMW_SET_ERROR_MSG("rmw_init_publisher_allocation: unimplemented"); return RMW_RET_UNSUPPORTED; } @@ -178,6 +183,8 @@ rmw_ret_t rmw_fini_publisher_allocation( rmw_publisher_allocation_t * allocation) { + static_cast(allocation); + RMW_SET_ERROR_MSG("rmw_fini_publisher_allocation: unimplemented"); return RMW_RET_UNSUPPORTED; } @@ -191,7 +198,106 @@ rmw_create_publisher( const rmw_qos_profile_t * qos_profile, const rmw_publisher_options_t * publisher_options) { - return nullptr; + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + rmw_zenoh_identifier, + return nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_support, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) { + RMW_SET_ERROR_MSG("topic_name argument is an empty string"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_profile, nullptr); + if (!qos_profile->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic name: %s", reason); + return nullptr; + } + } + // Adapt any 'best available' QoS options + // rmw_qos_profile_t adapted_qos_profile = *qos_profile; + // rmw_ret_t ret = rmw_dds_common::qos_profile_get_best_available_for_topic_publisher( + // node, topic_name, &adapted_qos_profile, rmw_get_subscriptions_info_by_topic); + // if (RMW_RET_OK != ret) { + // return nullptr; + // } + RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); + if (publisher_options->require_unique_network_flow_endpoints == + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED) + { + RMW_SET_ERROR_MSG( + "Strict requirement on unique network flow endpoints for publishers not supported"); + return nullptr; + } + + RMW_CHECK_FOR_NULL_WITH_MSG( + node->context, + "expected initialized context", + return nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG( + node->context->impl, + "expected initialized context impl", + return nullptr); + + rmw_context_impl_s * context_impl = static_cast( + node->context->impl); + RMW_CHECK_FOR_NULL_WITH_MSG( + context_impl, + "unable to get rmw_context_impl_s", + return nullptr); + if (!z_check(context_impl->session)) { + RMW_SET_ERROR_MSG("zenoh session is invalid"); + return nullptr; + } + + // Create the publisher. + rmw_publisher_t * rmw_publisher = rmw_publisher_allocate(); + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_publisher, nullptr); + // Get typed pointer to implementation specific publisher data struct + auto publisher_data = static_cast(rmw_publisher->data); + if (publisher_data == nullptr) { + RMW_SET_ERROR_MSG("unable to cast publisher data into rmw_publisher_data_t"); + return nullptr; + } + // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate + // a z_publisher_put_options struct instead of passing NULL to this function. + publisher_data->pub = z_declare_publisher( + z_loan(context_impl->session), + z_keyexpr(topic_name), + NULL + ); + if (!z_check(publisher_data->pub)) { + RMW_SET_ERROR_MSG("unable to create publisher"); + return nullptr; + } + + auto cleanup_rmw_publisher = rcpputils::make_scope_exit( + [rmw_publisher]() { + auto publisher_data = static_cast(rmw_publisher->data); + z_undeclare_publisher(z_move(publisher_data->pub)); + rmw_free(const_cast(rmw_publisher->topic_name)); + rmw_publisher_free(rmw_publisher); + }); + + rmw_publisher->implementation_identifier = rmw_zenoh_identifier; + rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_publisher->topic_name, nullptr); + memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); + rmw_publisher->options = *publisher_options; + // TODO(yadunund): Update this. + rmw_publisher->can_loan_messages = false; + + cleanup_rmw_publisher.cancel(); + return rmw_publisher; } //============================================================================== @@ -199,7 +305,30 @@ rmw_create_publisher( rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + rmw_ret_t ret = RMW_RET_OK; + auto publisher_data = static_cast(publisher->data); + if (publisher_data != nullptr) { + if (!z_undeclare_publisher(z_move(publisher_data->pub))) { + RMW_SET_ERROR_MSG("failed to undeclare pub"); + ret = RMW_RET_ERROR; + } + } + rmw_free(const_cast(publisher->topic_name)); + rmw_publisher_free(publisher); + return ret; } //============================================================================== @@ -210,6 +339,9 @@ rmw_borrow_loaned_message( const rosidl_message_type_support_t * type_support, void ** ros_message) { + static_cast(publisher); + static_cast(type_support); + static_cast(ros_message); return RMW_RET_UNSUPPORTED; } @@ -220,6 +352,8 @@ rmw_return_loaned_message_from_publisher( const rmw_publisher_t * publisher, void * loaned_message) { + static_cast(publisher); + static_cast(loaned_message); return RMW_RET_UNSUPPORTED; } @@ -231,7 +365,31 @@ rmw_publish( const void * ros_message, rmw_publisher_allocation_t * allocation) { + static_cast(allocation); + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher, "publisher handle is null", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, publisher->implementation_identifier, rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_FOR_NULL_WITH_MSG( + ros_message, "ros message handle is null", + return RMW_RET_INVALID_ARGUMENT); + + auto publisher_data = static_cast(publisher->data); + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher_data, "publisher_data is null", + return RMW_RET_INVALID_ARGUMENT); + + // Returns 0 if success. + // int8_t ret = z_publisher_put( + // z_loan(publisher_data->pub), + // const uint8_t *payload, + // uintptr_t len, + // NULL) + return RMW_RET_UNSUPPORTED; + } //============================================================================== @@ -354,7 +512,7 @@ rmw_create_subscription( const rmw_node_t * node, const rosidl_message_type_support_t * type_support, const char * topic_name, - const rmw_qos_profile_t * qos_policies, + const rmw_qos_profile_t * qos_profile, const rmw_subscription_options_t * subscription_options) { return nullptr; @@ -515,7 +673,7 @@ rmw_create_client( const rmw_node_t * node, const rosidl_service_type_support_t * type_support, const char * service_name, - const rmw_qos_profile_t * qos_policies) + const rmw_qos_profile_t * qos_profile) { return nullptr; } From 78d67a788de69ebea1eb052d4b7790452fe9a524 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 7 Sep 2023 21:47:54 +0800 Subject: [PATCH 13/50] Use typesupport from fastrtps cpp Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 12 ++++++++++-- rmw_zenoh_cpp/package.xml | 3 +++ rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 7 +++++++ 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 4f89aaf6..2b2dff70 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -20,11 +20,15 @@ find_package(ament_cmake REQUIRED) find_package(fastcdr CONFIG REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) +find_package(rosidl_typesupport_fastrtps_c REQUIRED) +find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) find_package(rmw REQUIRED) +find_package(rmw_fastrtps_cpp REQUIRED) find_package(zenoh_c_vendor REQUIRED) find_package(zenohc REQUIRED) + add_library(rmw_zenoh_cpp SHARED src/rmw_get_node_info_and_types.cpp src/rmw_get_service_names_and_types.cpp @@ -39,7 +43,10 @@ target_link_libraries(rmw_zenoh_cpp PRIVATE rcpputils::rcpputils rcutils::rcutils + rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp rmw::rmw + ${rmw_fastrtps_cpp_LIBRARIES} zenohc::lib ) @@ -54,8 +61,9 @@ target_compile_definitions(rmw_zenoh_cpp ament_export_targets(export_rmw_zenoh_cpp) -register_rmw_implementation("c:rosidl_typesupport_introspection_c" - "cpp:rosidl_typesupport_introspection_cpp") +register_rmw_implementation( + "c:rosidl_typesupport_c:rosidl_typesupport_fastrtps_c:rosidl_typesupport_introspection_c" + "cpp:rosidl_typesupport_cpp:rosidl_typesupport_fastrtps_cpp:rosidl_typesupport_introspection_cpp") install( TARGETS rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index c0483935..ebee1630 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -14,7 +14,10 @@ fastcdr rcpputils rcutils + rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp rmw + rmw_fastrtps_cpp ament_cmake diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 4f049a64..d01e9a77 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -17,6 +17,8 @@ #include "rmw/rmw.h" +#include "rmw_fastrtps_cpp/TypeSupport.hpp" + #include "zenoh.h" /// Structs for various type erased data fields. @@ -25,6 +27,11 @@ struct rmw_publisher_data_t { // An owned publisher. z_owned_publisher_t pub; + + // Type support fields + const void * type_support_impl; + const char * typesupport_identifier; + rmw_fastrtps_cpp::TypeSupport * type_support; }; #endif // SRC__DETAIL__RMW_DATA_TYPES_HPP From 8821bf8a59f92ff2f5af56d733edd1a0f4b972e7 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 7 Sep 2023 23:16:14 +0800 Subject: [PATCH 14/50] Copy typesupport files Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 17 +- rmw_zenoh_cpp/package.xml | 4 +- .../src/detail/MessageTypeSupport.hpp | 32 ++++ .../src/detail/ServiceTypeSupport.hpp | 45 ++++++ rmw_zenoh_cpp/src/detail/TypeSupport.hpp | 60 +++++++ rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 4 +- .../src/detail/type_support_common.cpp | 146 ++++++++++++++++++ .../src/detail/type_support_common.hpp | 39 +++++ 8 files changed, 336 insertions(+), 11 deletions(-) create mode 100644 rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp create mode 100644 rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp create mode 100644 rmw_zenoh_cpp/src/detail/TypeSupport.hpp create mode 100644 rmw_zenoh_cpp/src/detail/type_support_common.cpp create mode 100644 rmw_zenoh_cpp/src/detail/type_support_common.hpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 2b2dff70..6661a698 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -18,10 +18,11 @@ add_compile_options(-Wno-unused-parameter) find_package(ament_cmake REQUIRED) find_package(fastcdr CONFIG REQUIRED) +find_package(fastrtps_cmake_module REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) -find_package(rosidl_typesupport_fastrtps_c REQUIRED) -find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) +find_package(rosidl_typesupport_zenoh_c REQUIRED) +find_package(rosidl_typesupport_zenoh_cpp REQUIRED) find_package(rmw REQUIRED) find_package(rmw_fastrtps_cpp REQUIRED) find_package(zenoh_c_vendor REQUIRED) @@ -30,6 +31,7 @@ find_package(zenohc REQUIRED) add_library(rmw_zenoh_cpp SHARED + src/detail/type_support_common.cpp src/rmw_get_node_info_and_types.cpp src/rmw_get_service_names_and_types.cpp src/rmw_get_topic_endpoint_info.cpp @@ -41,12 +43,12 @@ add_library(rmw_zenoh_cpp SHARED target_link_libraries(rmw_zenoh_cpp PRIVATE + fastcdr rcpputils::rcpputils rcutils::rcutils - rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c - rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp + rosidl_typesupport_zenoh_c::rosidl_typesupport_zenoh_c + rosidl_typesupport_zenoh_cpp::rosidl_typesupport_zenoh_cpp rmw::rmw - ${rmw_fastrtps_cpp_LIBRARIES} zenohc::lib ) @@ -62,8 +64,9 @@ target_compile_definitions(rmw_zenoh_cpp ament_export_targets(export_rmw_zenoh_cpp) register_rmw_implementation( - "c:rosidl_typesupport_c:rosidl_typesupport_fastrtps_c:rosidl_typesupport_introspection_c" - "cpp:rosidl_typesupport_cpp:rosidl_typesupport_fastrtps_cpp:rosidl_typesupport_introspection_cpp") + "c:rosidl_typesupport_c:rosidl_typesupport_zenoh_c" + "cpp:rosidl_typesupport_cpp:rosidl_typesupport_zenoh_cpp" +) install( TARGETS rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index ebee1630..13eb27d5 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -14,8 +14,8 @@ fastcdr rcpputils rcutils - rosidl_typesupport_fastrtps_c - rosidl_typesupport_fastrtps_cpp + rosidl_typesupport_zenoh_c + rosidl_typesupport_zenoh_cpp rmw rmw_fastrtps_cpp diff --git a/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp b/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp new file mode 100644 index 00000000..de108c6c --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp @@ -0,0 +1,32 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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. + +// This file is originally from: +// https://github.com/ros2/rmw_fastrtps/blob/b13e134cea2852aba210299bef6f4df172d9a0e3/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp + +#ifndef SRC__DETAIL__MESSAGETYPESUPPORT_HPP +#define SRC__DETAIL__MESSAGETYPESUPPORT_HPP + +#include "rosidl_typesupport_zenoh_cpp/message_type_support.h" +#include "TypeSupport.hpp" + +///============================================================================== +class MessageTypeSupport : public TypeSupport +{ +public: + explicit MessageTypeSupport(const message_type_support_callbacks_t * members); +}; + +#endif // SRC__DETAIL__MESSAGETYPESUPPORT_HPP diff --git a/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp b/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp new file mode 100644 index 00000000..33970a55 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp @@ -0,0 +1,45 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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. + +// This file is originally from: +// https://github.com/ros2/rmw_fastrtps/blob/b13e134cea2852aba210299bef6f4df172d9a0e3/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp + +#ifndef SRC__DETAIL__SERVICETYPESUPPORT_HPP +#define SRC__DETAIL__SERVICETYPESUPPORT_HPP + +#include "rosidl_typesupport_zenoh_cpp/message_type_support.h" +#include "rosidl_typesupport_zenoh_cpp/service_type_support.h" +#include "TypeSupport.hpp" + +///============================================================================== +class ServiceTypeSupport : public TypeSupport +{ +protected: + ServiceTypeSupport(); +}; + +class RequestTypeSupport : public ServiceTypeSupport +{ +public: + explicit RequestTypeSupport(const service_type_support_callbacks_t * members); +}; + +class ResponseTypeSupport : public ServiceTypeSupport +{ +public: + explicit ResponseTypeSupport(const service_type_support_callbacks_t * members); +}; + +#endif // SRC__DETAIL__SERVICETYPESUPPORT_HPP diff --git a/rmw_zenoh_cpp/src/detail/TypeSupport.hpp b/rmw_zenoh_cpp/src/detail/TypeSupport.hpp new file mode 100644 index 00000000..f1db2129 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/TypeSupport.hpp @@ -0,0 +1,60 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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. + +// This file is originally from: +// https://github.com/ros2/rmw_fastrtps/blob/b13e134cea2852aba210299bef6f4df172d9a0e3/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp + +#ifndef SRC__DETAIL__TYPESUPPORT_HPP +#define SRC__DETAIL__TYPESUPPORT_HPP + +// #include + +#include +#include +#include +#include + +#include "rosidl_typesupport_zenoh_cpp/message_type_support.h" + +///============================================================================== +class TypeSupport +{ +public: + size_t getEstimatedSerializedSize(const void * ros_message); + + bool serializeROSmessage( + const void * ros_message, + eprosima::fastcdr::Cdr & ser, + const void * impl) const; + + bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, + void * ros_message, + const void * impl) const; + +protected: + TypeSupport(); + + void set_members(const message_type_support_callbacks_t * members); + +private: + const message_type_support_callbacks_t * members_; + bool has_data_; + bool max_size_bound_; + + size_t type_size_; +}; + +#endif // SRC__DETAIL__TYPESUPPORT_HPP diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index d01e9a77..57cc0c15 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -17,7 +17,7 @@ #include "rmw/rmw.h" -#include "rmw_fastrtps_cpp/TypeSupport.hpp" +#include "TypeSupport.hpp" #include "zenoh.h" @@ -31,7 +31,7 @@ struct rmw_publisher_data_t // Type support fields const void * type_support_impl; const char * typesupport_identifier; - rmw_fastrtps_cpp::TypeSupport * type_support; + TypeSupport * type_support; }; #endif // SRC__DETAIL__RMW_DATA_TYPES_HPP diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.cpp b/rmw_zenoh_cpp/src/detail/type_support_common.cpp new file mode 100644 index 00000000..044473aa --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/type_support_common.cpp @@ -0,0 +1,146 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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. + +// This file is originally from: +// https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp + +#include + +#include "rmw/error_handling.h" +#include "type_support_common.hpp" + +///============================================================================== +TypeSupport::TypeSupport() +{ + max_size_bound_ = false; +} + +///============================================================================== +void TypeSupport::set_members(const message_type_support_callbacks_t * members) +{ + members_ = members; + + // Fully bound by default + max_size_bound_ = true; + auto data_size = static_cast(members->max_serialized_size(max_size_bound_)); + + // A fully bound message of size 0 is an empty message + if (max_size_bound_ && (data_size == 0) ) { + has_data_ = false; + ++data_size; // Dummy byte + } else { + has_data_ = true; + } + + // Total size is encapsulation size + data size + type_size_ = 4 + data_size; +} + +///============================================================================== +size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message) +{ + if (max_size_bound_) { + return type_size_; + } + + assert(ros_message); + + // Encapsulation size + message size + return 4 + members_->get_serialized_size(ros_message); +} + +///============================================================================== +bool TypeSupport::serializeROSmessage( + const void * ros_message, + eprosima::fastcdr::Cdr & ser, + const void * impl) const +{ + assert(ros_message); + assert(impl); + + // Serialize encapsulation + ser.serialize_encapsulation(); + + // If type is not empty, serialize message + if (has_data_) { + auto callbacks = static_cast(impl); + return callbacks->cdr_serialize(ros_message, ser); + } + + // Otherwise, add a dummy byte + ser << (uint8_t)0; + return true; +} + +///============================================================================== +bool TypeSupport::deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, + void * ros_message, + const void * impl) const +{ + assert(ros_message); + assert(impl); + + // Deserialize encapsulation. + deser.read_encapsulation(); + + // If type is not empty, deserialize message + if (has_data_) { + auto callbacks = static_cast(impl); + return callbacks->cdr_deserialize(deser, ros_message); + } + + // Otherwise, consume dummy byte + uint8_t dump = 0; + deser >> dump; + (void)dump; + + return true; +} + +///============================================================================== +MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * members) +{ + assert(members); + + set_members(members); +} + +///============================================================================== +ServiceTypeSupport::ServiceTypeSupport() +{ +} + +///============================================================================== +RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * members) +{ + assert(members); + + auto msg = static_cast( + members->request_members_->data); + + set_members(msg); +} + +///============================================================================== +ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t * members) +{ + assert(members); + + auto msg = static_cast( + members->response_members_->data); + + set_members(msg); +} diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.hpp b/rmw_zenoh_cpp/src/detail/type_support_common.hpp new file mode 100644 index 00000000..99986e1a --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/type_support_common.hpp @@ -0,0 +1,39 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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. + +// This file is originally from: +// https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp + +#ifndef SRC__DETAIL__TYPE_SUPPORT_COMMON_HPP +#define SRC__DETAIL__TYPE_SUPPORT_COMMON_HPP + +#include +#include + +#include "rmw/error_handling.h" + +#include "TypeSupport.hpp" +#include "MessageTypeSupport.hpp" +#include "ServiceTypeSupport.hpp" + +#include "rosidl_typesupport_zenoh_c/identifier.h" +#include "rosidl_typesupport_zenoh_cpp/identifier.hpp" +#include "rosidl_typesupport_zenoh_cpp/message_type_support.h" +#include "rosidl_typesupport_zenoh_cpp/service_type_support.h" + +#define RMW_ZENOH_CPP_TYPESUPPORT_C rosidl_typesupport_zenoh_c__identifier +#define RMW_ZENOH_CPP_TYPESUPPORT_CPP rosidl_typesupport_zenoh_cpp::typesupport_identifier + +#endif // SRC__DETAIL__TYPE_SUPPORT_COMMON_HPP From bce494d96e3c7ccaa73c2b4980eb05aa0f519a65 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sat, 9 Sep 2023 02:07:34 +0800 Subject: [PATCH 15/50] publish messages Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 1 + rmw_zenoh_cpp/src/detail/identifier.cpp | 17 ++ rmw_zenoh_cpp/src/detail/identifier.hpp | 2 +- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 3 + rmw_zenoh_cpp/src/rmw_zenoh.cpp | 190 ++++++++++++++++++-- 5 files changed, 202 insertions(+), 11 deletions(-) create mode 100644 rmw_zenoh_cpp/src/detail/identifier.cpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 6661a698..bab74bfa 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(zenohc REQUIRED) add_library(rmw_zenoh_cpp SHARED + src/detail/identifier.cpp src/detail/type_support_common.cpp src/rmw_get_node_info_and_types.cpp src/rmw_get_service_names_and_types.cpp diff --git a/rmw_zenoh_cpp/src/detail/identifier.cpp b/rmw_zenoh_cpp/src/detail/identifier.cpp new file mode 100644 index 00000000..ce5f86d9 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/identifier.cpp @@ -0,0 +1,17 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 "identifier.hpp" + +const char * const rmw_zenoh_identifier = "rmw_zenoh_cpp"; diff --git a/rmw_zenoh_cpp/src/detail/identifier.hpp b/rmw_zenoh_cpp/src/detail/identifier.hpp index cb850960..752968b5 100644 --- a/rmw_zenoh_cpp/src/detail/identifier.hpp +++ b/rmw_zenoh_cpp/src/detail/identifier.hpp @@ -15,6 +15,6 @@ #ifndef SRC__DETAIL__IDENTIFIER_HPP #define SRC__DETAIL__IDENTIFIER_HPP -extern inline const char * const rmw_zenoh_identifier = "rmw_zenoh_cpp"; +extern const char * const rmw_zenoh_identifier; #endif // SRC__DETAIL__IDENTIFIER_HPP diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 57cc0c15..cce7965f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -32,6 +32,9 @@ struct rmw_publisher_data_t const void * type_support_impl; const char * typesupport_identifier; TypeSupport * type_support; + + // Context for memory allocation for messages. + rmw_context_t * context; }; #endif // SRC__DETAIL__RMW_DATA_TYPES_HPP diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index b5c7e59f..b2534456 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -16,6 +16,10 @@ #include "detail/rmw_context_impl.hpp" #include "detail/rmw_data_types.hpp" #include "detail/serialization_format.hpp" +#include "detail/type_support_common.hpp" + +#include +#include #include "rcpputils/scope_exit.hpp" @@ -24,7 +28,9 @@ #include "rcutils/types.h" #include "rmw/allocators.h" +#include "rmw/dynamic_message_type_support.h" #include "rmw/error_handling.h" +#include "rmw/features.h" #include "rmw/impl/cpp/macros.hpp" #include "rmw/ret_types.h" #include "rmw/rmw.h" @@ -49,6 +55,23 @@ rmw_get_serialization_format(void) return rmw_zenoh_serialization_format; } +//============================================================================== +bool rmw_feature_supported(rmw_feature_t feature) +{ + switch (feature) { + case RMW_FEATURE_MESSAGE_INFO_PUBLICATION_SEQUENCE_NUMBER: + return false; + case RMW_FEATURE_MESSAGE_INFO_RECEPTION_SEQUENCE_NUMBER: + return false; + case RMW_MIDDLEWARE_SUPPORTS_TYPE_DISCOVERY: + return true; + case RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_MESSAGE: + return false; + default: + return false; + } +} + //============================================================================== /// Create a node and return a handle to that node. rmw_node_t * @@ -193,7 +216,7 @@ rmw_fini_publisher_allocation( rmw_publisher_t * rmw_create_publisher( const rmw_node_t * node, - const rosidl_message_type_support_t * type_support, + const rosidl_message_type_support_t * type_supports, const char * topic_name, const rmw_qos_profile_t * qos_profile, const rmw_publisher_options_t * publisher_options) @@ -204,7 +227,7 @@ rmw_create_publisher( node->implementation_identifier, rmw_zenoh_identifier, return nullptr); - RMW_CHECK_ARGUMENT_FOR_NULL(type_support, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); if (0 == strlen(topic_name)) { RMW_SET_ERROR_MSG("topic_name argument is an empty string"); @@ -239,6 +262,26 @@ rmw_create_publisher( return nullptr; } + // Get the RMW type support. + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, RMW_ZENOH_CPP_TYPESUPPORT_C); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle(type_supports, RMW_ZENOH_CPP_TYPESUPPORT_CPP); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } + } + RMW_CHECK_FOR_NULL_WITH_MSG( node->context, "expected initialized context", @@ -247,7 +290,6 @@ rmw_create_publisher( node->context->impl, "expected initialized context impl", return nullptr); - rmw_context_impl_s * context_impl = static_cast( node->context->impl); RMW_CHECK_FOR_NULL_WITH_MSG( @@ -279,6 +321,18 @@ rmw_create_publisher( RMW_SET_ERROR_MSG("unable to create publisher"); return nullptr; } + publisher_data->typesupport_identifier = type_support->typesupport_identifier; + publisher_data->type_support_impl = type_support->data; + auto callbacks = static_cast(type_support->data); + rcutils_allocator_t * allocator = &node->context->options.allocator; + publisher_data->type_support = static_cast( + allocator->allocate(sizeof(MessageTypeSupport), allocator->state)); + new(publisher_data->type_support) MessageTypeSupport(callbacks); + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher_data->type_support, + "Failed to allocate MessageTypeSupport", + return nullptr); + publisher_data->context = node->context; auto cleanup_rmw_publisher = rcpputils::make_scope_exit( [rmw_publisher]() { @@ -331,6 +385,39 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) return ret; } +//============================================================================== +rmw_ret_t +rmw_take_dynamic_message( + const rmw_subscription_t * subscription, + rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +rmw_ret_t +rmw_take_dynamic_message_with_info( + const rmw_subscription_t * subscription, + rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + return RMW_RET_UNSUPPORTED; +} + +//============================================================================== +rmw_ret_t +rmw_serialization_support_init( + const char * serialization_lib_name, + rcutils_allocator_t * allocator, + rosidl_dynamic_typesupport_serialization_support_t * serialization_support) +{ + return RMW_RET_UNSUPPORTED; +} + //============================================================================== /// Borrow a loaned ROS message. rmw_ret_t @@ -381,15 +468,53 @@ rmw_publish( publisher_data, "publisher_data is null", return RMW_RET_INVALID_ARGUMENT); + // Assign allocator. + rcutils_allocator_t * allocator = + &(static_cast(publisher->data)->context->options.allocator); + + // Serialize data. + size_t max_data_length = (static_cast(publisher->data) + ->type_support->getEstimatedSerializedSize(ros_message)); + + // Init serialized message byte array + char * msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); + + // Object that manages the raw buffer + eprosima::fastcdr::FastBuffer fastbuffer(msg_bytes, max_data_length); + + // Object that serializes the data + eprosima::fastcdr::Cdr ser( + fastbuffer, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (!publisher_data->type_support->serializeROSmessage( + ros_message, + ser, + publisher_data->type_support_impl)) + { + RMW_SET_ERROR_MSG("could not serialize ROS message"); + allocator->deallocate(msg_bytes, allocator->state); + return RMW_RET_ERROR; + } + + const size_t data_length = ser.getSerializedDataLength(); + // Returns 0 if success. - // int8_t ret = z_publisher_put( - // z_loan(publisher_data->pub), - // const uint8_t *payload, - // uintptr_t len, - // NULL) + int8_t ret = z_publisher_put( + z_loan(publisher_data->pub), + (const uint8_t *)msg_bytes, + data_length, + NULL); - return RMW_RET_UNSUPPORTED; + allocator->deallocate(msg_bytes, allocator->state); + + if (ret) + { + RMW_SET_ERROR_MSG("unable to publish message"); + return RMW_RET_ERROR; + } + return RMW_RET_OK; } //============================================================================== @@ -400,6 +525,9 @@ rmw_publish_loaned_message( void * ros_message, rmw_publisher_allocation_t * allocation) { + static_cast(publisher); + static_cast(ros_message); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -410,6 +538,8 @@ rmw_publisher_count_matched_subscriptions( const rmw_publisher_t * publisher, size_t * subscription_count) { + static_cast(publisher); + static_cast(subscription_count); return RMW_RET_UNSUPPORTED; } @@ -420,6 +550,8 @@ rmw_publisher_get_actual_qos( const rmw_publisher_t * publisher, rmw_qos_profile_t * qos) { + static_cast(publisher); + static_cast(qos); return RMW_RET_UNSUPPORTED; } @@ -431,7 +563,45 @@ rmw_publish_serialized_message( const rmw_serialized_message_t * serialized_message, rmw_publisher_allocation_t * allocation) { - return RMW_RET_UNSUPPORTED; + static_cast(allocation); + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher, "publisher handle is null", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, publisher->implementation_identifier, rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_FOR_NULL_WITH_MSG( + serialized_message, "serialized message handle is null", + return RMW_RET_INVALID_ARGUMENT); + + auto publisher_data = static_cast(publisher->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(publisher_data, "publisher data pointer is null", return RMW_RET_ERROR); + + eprosima::fastcdr::FastBuffer buffer( + reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); + eprosima::fastcdr::Cdr ser( + buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + if (!ser.jump(serialized_message->buffer_length)) { + RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); + return RMW_RET_ERROR; + } + + const size_t data_length = ser.getSerializedDataLength(); + + // Returns 0 if success. + int8_t ret = z_publisher_put( + z_loan(publisher_data->pub), + serialized_message->buffer, + data_length, + NULL); + + if (ret) + { + RMW_SET_ERROR_MSG("unable to publish message"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; } //============================================================================== From 58cf48cdc228246b9b174d2f2094360c1b5f6aee Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sat, 9 Sep 2023 02:36:49 +0800 Subject: [PATCH 16/50] Add missing apis Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 5 ++ rmw_zenoh_cpp/package.xml | 1 + rmw_zenoh_cpp/src/rmw_event.cpp | 70 +++++++++++++++++++ .../src/rmw_get_network_flow_endpoints.cpp | 47 +++++++++++++ rmw_zenoh_cpp/src/rmw_qos.cpp | 34 +++++++++ 5 files changed, 157 insertions(+) create mode 100644 rmw_zenoh_cpp/src/rmw_event.cpp create mode 100644 rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp create mode 100644 rmw_zenoh_cpp/src/rmw_qos.cpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index bab74bfa..313cd967 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(rcutils REQUIRED) find_package(rosidl_typesupport_zenoh_c REQUIRED) find_package(rosidl_typesupport_zenoh_cpp REQUIRED) find_package(rmw REQUIRED) +find_package(rmw_dds_common REQUIRED) find_package(rmw_fastrtps_cpp REQUIRED) find_package(zenoh_c_vendor REQUIRED) find_package(zenohc REQUIRED) @@ -33,12 +34,15 @@ find_package(zenohc REQUIRED) add_library(rmw_zenoh_cpp SHARED src/detail/identifier.cpp src/detail/type_support_common.cpp + src/rmw_event.cpp + src/rmw_get_network_flow_endpoints.cpp src/rmw_get_node_info_and_types.cpp src/rmw_get_service_names_and_types.cpp src/rmw_get_topic_endpoint_info.cpp src/rmw_get_topic_names_and_types.cpp src/rmw_init_options.cpp src/rmw_init.cpp + src/rmw_qos.cpp src/rmw_zenoh.cpp ) @@ -50,6 +54,7 @@ target_link_libraries(rmw_zenoh_cpp rosidl_typesupport_zenoh_c::rosidl_typesupport_zenoh_c rosidl_typesupport_zenoh_cpp::rosidl_typesupport_zenoh_cpp rmw::rmw + ${rmw_dds_common_TARGETS} zenohc::lib ) diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index 13eb27d5..4d312ffd 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -17,6 +17,7 @@ rosidl_typesupport_zenoh_c rosidl_typesupport_zenoh_cpp rmw + rmw_dds_common rmw_fastrtps_cpp diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp new file mode 100644 index 00000000..4dcd418d --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -0,0 +1,70 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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/event.h" + +extern "C" +{ +///============================================================================== +/// Initialize a rmw subscription event +rmw_ret_t +rmw_publisher_event_init( + rmw_event_t * rmw_event, + const rmw_publisher_t * publisher, + rmw_event_type_t event_type) +{ + (void) rmw_event; + (void) publisher; + (void) event_type; + return RMW_RET_UNSUPPORTED; +} + +///============================================================================== +/// Take an event from the event handle. +rmw_ret_t +rmw_subscription_event_init( + rmw_event_t * rmw_event, + const rmw_subscription_t * subscription, + rmw_event_type_t event_type) +{ + (void) rmw_event; + (void) subscription; + (void) event_type; + return RMW_RET_UNSUPPORTED; +} + +///============================================================================== +rmw_ret_t +rmw_take_event( + const rmw_event_t * event_handle, + void * event_info, + bool * taken) +{ + (void) event_handle; + (void) event_info; + (void) taken; + return RMW_RET_UNSUPPORTED; +} + + +///============================================================================== +/// Finalize an rmw_event_t. +rmw_ret_t +rmw_event_fini(rmw_event_t * event) +{ + (void) event; + return RMW_RET_UNSUPPORTED; +} + +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp b/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp new file mode 100644 index 00000000..5e0cd285 --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp @@ -0,0 +1,47 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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" + +extern "C" +{ +///============================================================================== +/// Get network flow endpoints of a publisher +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) +{ + (void) publisher; + (void) allocator; + (void) network_flow_endpoint_array; + return RMW_RET_UNSUPPORTED; +} + +///============================================================================== +/// Get network flow endpoints of a subscription +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) +{ + (void) subscription; + (void) allocator; + (void) network_flow_endpoint_array; + return RMW_RET_UNSUPPORTED; +} + +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_qos.cpp b/rmw_zenoh_cpp/src/rmw_qos.cpp new file mode 100644 index 00000000..437aa4e5 --- /dev/null +++ b/rmw_zenoh_cpp/src/rmw_qos.cpp @@ -0,0 +1,34 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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_dds_common/qos.hpp" +#include "rmw/types.h" +#include "rmw/qos_profiles.h" + +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) +{ + return rmw_dds_common::qos_profile_check_compatible( + publisher_profile, subscription_profile, compatibility, reason, reason_size); +} + +} // extern "C" From 923e7e2a532ced48e92081cc7cad57b4a7a263e4 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sat, 9 Sep 2023 03:07:10 +0800 Subject: [PATCH 17/50] initialize guard condition within context Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 1 + rmw_zenoh_cpp/src/detail/guard_condition.cpp | 70 ++++++++++++++++++++ rmw_zenoh_cpp/src/detail/guard_condition.hpp | 51 ++++++++++++++ rmw_zenoh_cpp/src/rmw_init.cpp | 17 +++++ 4 files changed, 139 insertions(+) create mode 100644 rmw_zenoh_cpp/src/detail/guard_condition.cpp create mode 100644 rmw_zenoh_cpp/src/detail/guard_condition.hpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 313cd967..f28ee75d 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -33,6 +33,7 @@ find_package(zenohc REQUIRED) add_library(rmw_zenoh_cpp SHARED src/detail/identifier.cpp + src/detail/guard_condition.cpp src/detail/type_support_common.cpp src/rmw_event.cpp src/rmw_get_network_flow_endpoints.cpp diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.cpp b/rmw_zenoh_cpp/src/detail/guard_condition.cpp new file mode 100644 index 00000000..fe5b058b --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/guard_condition.cpp @@ -0,0 +1,70 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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 "guard_condition.hpp" + +///============================================================================== +GuardCondition::GuardCondition() +: hasTriggered_(false), + conditionMutex_(nullptr), + conditionVariable_(nullptr) {} + +///============================================================================== +void GuardCondition::trigger() +{ + std::lock_guard lock(internalMutex_); + + if (conditionMutex_ != nullptr) { + std::unique_lock clock(*conditionMutex_); + // the change to hasTriggered_ needs to be mutually exclusive with + // rmw_wait() which checks hasTriggered() and decides if wait() needs to + // be called + hasTriggered_ = true; + clock.unlock(); + conditionVariable_->notify_one(); + } else { + hasTriggered_ = true; + } +} + +///============================================================================== +void GuardCondition::attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) +{ + std::lock_guard lock(internalMutex_); + conditionMutex_ = conditionMutex; + conditionVariable_ = conditionVariable; +} + +///============================================================================== +void GuardCondition::detachCondition() +{ + std::lock_guard lock(internalMutex_); + conditionMutex_ = nullptr; + conditionVariable_ = nullptr; +} + +///============================================================================== +bool GuardCondition::hasTriggered() +{ + return hasTriggered_; +} + +///============================================================================== +bool GuardCondition::getHasTriggered() +{ + bool ret = hasTriggered_; + hasTriggered_ = false; + return ret; +} diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.hpp b/rmw_zenoh_cpp/src/detail/guard_condition.hpp new file mode 100644 index 00000000..5af51f84 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/guard_condition.hpp @@ -0,0 +1,51 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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 SRC__DETAIL__GUARDCONDITION_HPP +#define SRC__DETAIL__GUARDCONDITION_HPP + +#include +#include +#include +#include +#include +#include + +///============================================================================== +class GuardCondition +{ +public: + GuardCondition(); + + void trigger(); + + void attachCondition( + std::mutex * conditionMutex, + std::condition_variable * conditionVariable); + + void detachCondition(); + + bool hasTriggered(); + + bool getHasTriggered(); + +private: + std::mutex internalMutex_; + std::atomic_bool hasTriggered_; + std::mutex * conditionMutex_; + std::condition_variable * conditionVariable_; +}; + +#endif // SRC__DETAIL__GUARDCONDITION_HPP diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index ca285b0a..4ac3b2e3 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "detail/guard_condition.hpp" #include "detail/identifier.hpp" #include "detail/rmw_context_impl.hpp" @@ -105,6 +106,22 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } + // Initialize the guard condition. + // context->impl->graph_guard_condition = static_cast( + // option->allocator.allocate(sizeof(rmw_guard_condition_t), options->allocator.state)); + + context->impl->graph_guard_condition = rmw_guard_condition_allocate(); + context->impl->graph_guard_condition->implementation_identifier = rmw_zenoh_identifier; + context->impl->graph_guard_condition->data = new GuardCondition(); + + + if (!context->impl->graph_guard_condition) { + if (context->impl->graph_guard_condition->data) { + delete static_cast(context->impl->graph_guard_condition->data); + } + rmw_guard_condition_free(context->impl->graph_guard_condition); + } + cleanup_impl.cancel(); restore_context.cancel(); return RMW_RET_OK; From fe522a2fdee72f8d7e36b60cc6a78a067eba7512 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sun, 10 Sep 2023 20:58:01 +0800 Subject: [PATCH 18/50] Use fastrtps typesupport Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 18 +++-- rmw_zenoh_cpp/package.xml | 6 +- .../src/detail/MessageTypeSupport.hpp | 2 +- .../src/detail/ServiceTypeSupport.hpp | 4 +- rmw_zenoh_cpp/src/detail/TypeSupport.hpp | 22 +++--- rmw_zenoh_cpp/src/detail/guard_condition.cpp | 4 +- .../src/detail/type_support_common.cpp | 74 +++++++++++++------ .../src/detail/type_support_common.hpp | 42 +++++++++-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 36 ++++++--- 9 files changed, 139 insertions(+), 69 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index f28ee75d..b6a00b5d 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -21,11 +21,11 @@ find_package(fastcdr CONFIG REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) -find_package(rosidl_typesupport_zenoh_c REQUIRED) -find_package(rosidl_typesupport_zenoh_cpp REQUIRED) +find_package(rosidl_typesupport_fastrtps_c REQUIRED) +find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) find_package(rmw REQUIRED) find_package(rmw_dds_common REQUIRED) -find_package(rmw_fastrtps_cpp REQUIRED) +find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(zenoh_c_vendor REQUIRED) find_package(zenohc REQUIRED) @@ -52,10 +52,12 @@ target_link_libraries(rmw_zenoh_cpp fastcdr rcpputils::rcpputils rcutils::rcutils - rosidl_typesupport_zenoh_c::rosidl_typesupport_zenoh_c - rosidl_typesupport_zenoh_cpp::rosidl_typesupport_zenoh_cpp + rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp rmw::rmw ${rmw_dds_common_TARGETS} + ${rmw_fastrtps_shared_cpp_TARGETS} + ${rmw_fastrtps_shared_cpp_LIBRARIES} zenohc::lib ) @@ -71,9 +73,9 @@ target_compile_definitions(rmw_zenoh_cpp ament_export_targets(export_rmw_zenoh_cpp) register_rmw_implementation( - "c:rosidl_typesupport_c:rosidl_typesupport_zenoh_c" - "cpp:rosidl_typesupport_cpp:rosidl_typesupport_zenoh_cpp" -) + "c:rosidl_typesupport_c:rosidl_typesupport_fastrtps_c:rosidl_typesupport_introspection_c" + "cpp:rosidl_typesupport_cpp:rosidl_typesupport_fastrtps_cpp:rosidl_typesupport_introspection_cpp") + install( TARGETS rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index 4d312ffd..cb3c9be2 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -14,11 +14,11 @@ fastcdr rcpputils rcutils - rosidl_typesupport_zenoh_c - rosidl_typesupport_zenoh_cpp + rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp rmw rmw_dds_common - rmw_fastrtps_cpp + rmw_fastrtps_shared_cpp ament_cmake diff --git a/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp b/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp index de108c6c..97b5bc8e 100644 --- a/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp +++ b/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp @@ -19,7 +19,7 @@ #ifndef SRC__DETAIL__MESSAGETYPESUPPORT_HPP #define SRC__DETAIL__MESSAGETYPESUPPORT_HPP -#include "rosidl_typesupport_zenoh_cpp/message_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" #include "TypeSupport.hpp" ///============================================================================== diff --git a/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp b/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp index 33970a55..8134c908 100644 --- a/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp +++ b/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp @@ -19,8 +19,8 @@ #ifndef SRC__DETAIL__SERVICETYPESUPPORT_HPP #define SRC__DETAIL__SERVICETYPESUPPORT_HPP -#include "rosidl_typesupport_zenoh_cpp/message_type_support.h" -#include "rosidl_typesupport_zenoh_cpp/service_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" #include "TypeSupport.hpp" ///============================================================================== diff --git a/rmw_zenoh_cpp/src/detail/TypeSupport.hpp b/rmw_zenoh_cpp/src/detail/TypeSupport.hpp index f1db2129..79082f24 100644 --- a/rmw_zenoh_cpp/src/detail/TypeSupport.hpp +++ b/rmw_zenoh_cpp/src/detail/TypeSupport.hpp @@ -26,35 +26,31 @@ #include #include -#include "rosidl_typesupport_zenoh_cpp/message_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + ///============================================================================== -class TypeSupport +class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport { public: - size_t getEstimatedSerializedSize(const void * ros_message); + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const override; bool serializeROSmessage( - const void * ros_message, - eprosima::fastcdr::Cdr & ser, - const void * impl) const; + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const override; bool deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, - void * ros_message, - const void * impl) const; + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; -protected: TypeSupport(); +protected: void set_members(const message_type_support_callbacks_t * members); private: const message_type_support_callbacks_t * members_; bool has_data_; - bool max_size_bound_; - - size_t type_size_; }; #endif // SRC__DETAIL__TYPESUPPORT_HPP diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.cpp b/rmw_zenoh_cpp/src/detail/guard_condition.cpp index fe5b058b..d5f17139 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.cpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.cpp @@ -40,7 +40,9 @@ void GuardCondition::trigger() } ///============================================================================== -void GuardCondition::attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) +void GuardCondition::attachCondition( + std::mutex * conditionMutex, + std::condition_variable * conditionVariable) { std::lock_guard lock(internalMutex_); conditionMutex_ = conditionMutex; diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.cpp b/rmw_zenoh_cpp/src/detail/type_support_common.cpp index 044473aa..89dbfdff 100644 --- a/rmw_zenoh_cpp/src/detail/type_support_common.cpp +++ b/rmw_zenoh_cpp/src/detail/type_support_common.cpp @@ -24,7 +24,9 @@ ///============================================================================== TypeSupport::TypeSupport() { + m_isGetKeyDefined = false; max_size_bound_ = false; + is_plain_ = false; } ///============================================================================== @@ -32,12 +34,19 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members) { members_ = members; - // Fully bound by default - max_size_bound_ = true; - auto data_size = static_cast(members->max_serialized_size(max_size_bound_)); - - // A fully bound message of size 0 is an empty message - if (max_size_bound_ && (data_size == 0) ) { +#ifdef ROSIDL_TYPESUPPORT_FASTRTPS_HAS_PLAIN_TYPES + char bounds_info; + auto data_size = static_cast(members->max_serialized_size(bounds_info)); + max_size_bound_ = 0 != (bounds_info & ROSIDL_TYPESUPPORT_FASTRTPS_BOUNDED_TYPE); + is_plain_ = bounds_info == ROSIDL_TYPESUPPORT_FASTRTPS_PLAIN_TYPE; +#else + is_plain_ = true; + auto data_size = static_cast(members->max_serialized_size(is_plain_)); + max_size_bound_ = is_plain_; +#endif + + // A plain message of size 0 is an empty message + if (is_plain_ && (data_size == 0) ) { has_data_ = false; ++data_size; // Dummy byte } else { @@ -45,20 +54,25 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members) } // Total size is encapsulation size + data size - type_size_ = 4 + data_size; + m_typeSize = 4 + data_size; + // Account for RTPS submessage alignment + m_typeSize = (m_typeSize + 3) & ~3; } ///============================================================================== -size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message) +size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) const { - if (max_size_bound_) { - return type_size_; + if (is_plain_) { + return m_typeSize; } assert(ros_message); + assert(impl); + + auto callbacks = static_cast(impl); // Encapsulation size + message size - return 4 + members_->get_serialized_size(ros_message); + return 4 + callbacks->get_serialized_size(ros_message); } ///============================================================================== @@ -93,20 +107,27 @@ bool TypeSupport::deserializeROSmessage( assert(ros_message); assert(impl); - // Deserialize encapsulation. - deser.read_encapsulation(); - - // If type is not empty, deserialize message - if (has_data_) { - auto callbacks = static_cast(impl); - return callbacks->cdr_deserialize(deser, ros_message); + try { + // Deserialize encapsulation. + deser.read_encapsulation(); + + // If type is not empty, deserialize message + if (has_data_) { + auto callbacks = static_cast(impl); + return callbacks->cdr_deserialize(deser, ros_message); + } + + // Otherwise, consume dummy byte + uint8_t dump = 0; + deser >> dump; + (void)dump; + } catch (const eprosima::fastcdr::exception::Exception &) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Fast CDR exception deserializing message of type %s.", + getName()); + return false; } - // Otherwise, consume dummy byte - uint8_t dump = 0; - deser >> dump; - (void)dump; - return true; } @@ -115,6 +136,9 @@ MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * { assert(members); + std::string name = _create_type_name(members); + this->setName(name.c_str()); + set_members(members); } @@ -130,6 +154,8 @@ RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * auto msg = static_cast( members->request_members_->data); + std::string name = _create_type_name(msg); // + "Request_"; + this->setName(name.c_str()); set_members(msg); } @@ -141,6 +167,8 @@ ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t auto msg = static_cast( members->response_members_->data); + std::string name = _create_type_name(msg); // + "Response_"; + this->setName(name.c_str()); set_members(msg); } diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.hpp b/rmw_zenoh_cpp/src/detail/type_support_common.hpp index 99986e1a..4158e96b 100644 --- a/rmw_zenoh_cpp/src/detail/type_support_common.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support_common.hpp @@ -28,12 +28,42 @@ #include "MessageTypeSupport.hpp" #include "ServiceTypeSupport.hpp" -#include "rosidl_typesupport_zenoh_c/identifier.h" -#include "rosidl_typesupport_zenoh_cpp/identifier.hpp" -#include "rosidl_typesupport_zenoh_cpp/message_type_support.h" -#include "rosidl_typesupport_zenoh_cpp/service_type_support.h" +#include "identifier.hpp" -#define RMW_ZENOH_CPP_TYPESUPPORT_C rosidl_typesupport_zenoh_c__identifier -#define RMW_ZENOH_CPP_TYPESUPPORT_CPP rosidl_typesupport_zenoh_cpp::typesupport_identifier +#include "rosidl_typesupport_fastrtps_c/identifier.h" +#include "rosidl_typesupport_fastrtps_cpp/identifier.hpp" +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" +#include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" +#define RMW_FASTRTPS_CPP_TYPESUPPORT_C rosidl_typesupport_fastrtps_c__identifier +#define RMW_FASTRTPS_CPP_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp::typesupport_identifier + +#define RMW_ZENOH_CPP_TYPESUPPORT_C rosidl_typesupport_fastrtps_c__identifier +#define RMW_ZENOH_CPP_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp::typesupport_identifier + +inline std::string +_create_type_name( + std::string message_namespace, + std::string message_name) +{ + std::ostringstream ss; + if (!message_namespace.empty()) { + ss << message_namespace << "::"; + } + ss << "dds_::" << message_name << "_"; + return ss.str(); +} + +inline std::string +_create_type_name( + const message_type_support_callbacks_t * members) +{ + if (!members) { + RMW_SET_ERROR_MSG("members handle is null"); + return ""; + } + std::string message_namespace(members->message_namespace_); + std::string message_name(members->message_name_); + return _create_type_name(message_namespace, message_name); +} #endif // SRC__DETAIL__TYPE_SUPPORT_COMMON_HPP diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index b2534456..e61e1d77 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -264,11 +264,11 @@ rmw_create_publisher( // Get the RMW type support. const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, RMW_ZENOH_CPP_TYPESUPPORT_C); + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); - type_support = get_message_typesupport_handle(type_supports, RMW_ZENOH_CPP_TYPESUPPORT_CPP); + type_support = get_message_typesupport_handle(type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); if (!type_support) { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); @@ -305,11 +305,23 @@ rmw_create_publisher( rmw_publisher_t * rmw_publisher = rmw_publisher_allocate(); RMW_CHECK_ARGUMENT_FOR_NULL(rmw_publisher, nullptr); // Get typed pointer to implementation specific publisher data struct - auto publisher_data = static_cast(rmw_publisher->data); + // auto publisher_data = static_cast(rmw_publisher->data); + // if (publisher_data == nullptr) { + // RMW_SET_ERROR_MSG("unable to cast publisher data into rmw_publisher_data_t"); + // return nullptr; + // } + rcutils_allocator_t * allocator = &node->context->options.allocator; + auto publisher_data = static_cast( + allocator->allocate(sizeof(rmw_publisher_data_t), allocator->state)); if (publisher_data == nullptr) { - RMW_SET_ERROR_MSG("unable to cast publisher data into rmw_publisher_data_t"); + RMW_SET_ERROR_MSG("failed to allocate publisher data"); + allocator->deallocate(publisher_data, allocator->state); + allocator->deallocate(rmw_publisher, allocator->state); return nullptr; } + + // TODO(yadunund): Zenoh key cannot contain leading or trailing '/' so we strip them. + // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate // a z_publisher_put_options struct instead of passing NULL to this function. publisher_data->pub = z_declare_publisher( @@ -324,7 +336,6 @@ rmw_create_publisher( publisher_data->typesupport_identifier = type_support->typesupport_identifier; publisher_data->type_support_impl = type_support->data; auto callbacks = static_cast(type_support->data); - rcutils_allocator_t * allocator = &node->context->options.allocator; publisher_data->type_support = static_cast( allocator->allocate(sizeof(MessageTypeSupport), allocator->state)); new(publisher_data->type_support) MessageTypeSupport(callbacks); @@ -473,8 +484,9 @@ rmw_publish( &(static_cast(publisher->data)->context->options.allocator); // Serialize data. - size_t max_data_length = (static_cast(publisher->data) - ->type_support->getEstimatedSerializedSize(ros_message)); + size_t max_data_length = publisher_data->type_support->getEstimatedSerializedSize( + ros_message, + publisher_data->type_support_impl); // Init serialized message byte array char * msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); @@ -508,8 +520,7 @@ rmw_publish( allocator->deallocate(msg_bytes, allocator->state); - if (ret) - { + if (ret) { RMW_SET_ERROR_MSG("unable to publish message"); return RMW_RET_ERROR; } @@ -575,7 +586,9 @@ rmw_publish_serialized_message( return RMW_RET_INVALID_ARGUMENT); auto publisher_data = static_cast(publisher->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(publisher_data, "publisher data pointer is null", return RMW_RET_ERROR); + RCUTILS_CHECK_FOR_NULL_WITH_MSG( + publisher_data, "publisher data pointer is null", + return RMW_RET_ERROR); eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); @@ -595,8 +608,7 @@ rmw_publish_serialized_message( data_length, NULL); - if (ret) - { + if (ret) { RMW_SET_ERROR_MSG("unable to publish message"); return RMW_RET_ERROR; } From b82f4bbb95be15e70a992f6c6d18608169c1ab74 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sun, 10 Sep 2023 22:32:17 +0800 Subject: [PATCH 19/50] Initial wait set implementation Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 10 + rmw_zenoh_cpp/src/rmw_zenoh.cpp | 223 ++++++++++++++++++-- 2 files changed, 220 insertions(+), 13 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index cce7965f..d72d3085 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -15,6 +15,9 @@ #ifndef SRC__DETAIL__RMW_DATA_TYPES_HPP #define SRC__DETAIL__RMW_DATA_TYPES_HPP +#include +#include + #include "rmw/rmw.h" #include "TypeSupport.hpp" @@ -37,4 +40,11 @@ struct rmw_publisher_data_t rmw_context_t * context; }; +///============================================================================== +struct rmw_wait_set_data_t +{ + std::condition_variable condition; + std::mutex condition_mutex; +}; + #endif // SRC__DETAIL__RMW_DATA_TYPES_HPP diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index e61e1d77..c9d944ac 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "detail/guard_condition.hpp" #include "detail/identifier.hpp" #include "detail/rmw_context_impl.hpp" #include "detail/rmw_data_types.hpp" @@ -144,7 +145,6 @@ rmw_create_node( node->implementation_identifier = rmw_zenoh_identifier; node->context = context; return node; - } //============================================================================== @@ -321,12 +321,11 @@ rmw_create_publisher( } // TODO(yadunund): Zenoh key cannot contain leading or trailing '/' so we strip them. - // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate // a z_publisher_put_options struct instead of passing NULL to this function. publisher_data->pub = z_declare_publisher( z_loan(context_impl->session), - z_keyexpr(topic_name), + z_keyexpr(&topic_name[1]), NULL ); if (!z_check(publisher_data->pub)) { @@ -344,7 +343,7 @@ rmw_create_publisher( "Failed to allocate MessageTypeSupport", return nullptr); publisher_data->context = node->context; - + rmw_publisher->data = publisher_data; auto cleanup_rmw_publisher = rcpputils::make_scope_exit( [rmw_publisher]() { auto publisher_data = static_cast(rmw_publisher->data); @@ -386,7 +385,7 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) rmw_ret_t ret = RMW_RET_OK; auto publisher_data = static_cast(publisher->data); if (publisher_data != nullptr) { - if (!z_undeclare_publisher(z_move(publisher_data->pub))) { + if (z_undeclare_publisher(z_move(publisher_data->pub))) { RMW_SET_ERROR_MSG("failed to undeclare pub"); ret = RMW_RET_ERROR; } @@ -480,6 +479,8 @@ rmw_publish( return RMW_RET_INVALID_ARGUMENT); // Assign allocator. + // TODO(yadunund): Instead of storing the context in the publisher data to + // retrieve the allocator, use rmw_allocate(). rcutils_allocator_t * allocator = &(static_cast(publisher->data)->context->options.allocator); @@ -551,7 +552,8 @@ rmw_publisher_count_matched_subscriptions( { static_cast(publisher); static_cast(subscription_count); - return RMW_RET_UNSUPPORTED; + // TODO(yadunund): Fixme. + return RMW_RET_OK; } //============================================================================== @@ -563,7 +565,8 @@ rmw_publisher_get_actual_qos( { static_cast(publisher); static_cast(qos); - return RMW_RET_UNSUPPORTED; + // TODO(yadunund): Fixme. + return RMW_RET_OK; } //============================================================================== @@ -979,21 +982,38 @@ rmw_service_response_publisher_get_actual_qos( rmw_guard_condition_t * rmw_create_guard_condition(rmw_context_t * context) { - return nullptr; + rmw_guard_condition_t * guard_condition_handle = rmw_guard_condition_allocate(); + guard_condition_handle->implementation_identifier = rmw_zenoh_identifier; + guard_condition_handle->data = new GuardCondition(); + return guard_condition_handle; } /// Finalize a given guard condition handle, reclaim the resources, and deallocate the handle. rmw_ret_t rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(guard_condition, RMW_RET_INVALID_ARGUMENT); + + if (guard_condition->data) { + delete static_cast(guard_condition->data); + } + rmw_guard_condition_free(guard_condition); + return RMW_RET_OK; } //============================================================================== rmw_ret_t rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(guard_condition, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + guard_condition, + guard_condition->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + static_cast(guard_condition->data)->trigger(); + return RMW_RET_OK; } //============================================================================== @@ -1001,7 +1021,46 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition) rmw_wait_set_t * rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) { - return nullptr; + (void) max_conditions; + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + rmw_zenoh_identifier, + return nullptr); + + rmw_wait_set_t * wait_set = rmw_wait_set_allocate(); + + auto cleanup_wait_set = rcpputils::make_scope_exit( + [wait_set]() { + if (wait_set) { + if (wait_set->data) { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + static_cast(wait_set->data)->~rmw_wait_set_data_t(), + wait_set->data); + rmw_free(wait_set->data); + } + rmw_wait_set_free(wait_set); + } + }); + + // From here onward, error results in unrolling in the goto fail block. + if (!wait_set) { + RMW_SET_ERROR_MSG("failed to allocate wait set"); + return nullptr; + } + wait_set->implementation_identifier = rmw_zenoh_identifier; + wait_set->data = rmw_allocate(sizeof(rmw_wait_set_data_t)); + + // Invoke placement new + new(wait_set->data) rmw_wait_set_data_t; + if (!wait_set->data) { + RMW_SET_ERROR_MSG("failed to construct wait set info struct"); + return nullptr; + } + + cleanup_wait_set.cancel(); + return wait_set; } //============================================================================== @@ -1009,9 +1068,56 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t * wait_set) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(wait_set->data, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + wait_set, + wait_set->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + rmw_ret_t result = RMW_RET_OK; + auto wait_set_data = static_cast(wait_set->data); + + if (!wait_set_data) { + RMW_SET_ERROR_MSG("wait set info is null"); + return RMW_RET_ERROR; + } + std::mutex * conditionMutex = &wait_set_data->condition_mutex; + if (!conditionMutex) { + RMW_SET_ERROR_MSG("wait set mutex is null"); + return RMW_RET_ERROR; + } + + if (wait_set->data) { + if (wait_set_data) { + RMW_TRY_DESTRUCTOR( + wait_set_data->~rmw_wait_set_data_t(), + wait_set_data, result = RMW_RET_ERROR); + } + rmw_free(wait_set->data); + } + + rmw_wait_set_free(wait_set); + return result; } +//============================================================================== +namespace +{ +bool check_wait_conditions( + const rmw_subscriptions_t * subscriptions, + const rmw_guard_conditions_t * guard_conditions, + const rmw_services_t * services, + const rmw_clients_t * clients, + const rmw_events_t * events, + bool finalize) +{ + // TODO(yadunund): Fixme. + return true; +} + +} // namespace anonymous //============================================================================== /// Waits on sets of different entities and returns when one is ready. rmw_ret_t @@ -1024,7 +1130,98 @@ rmw_wait( rmw_wait_set_t * wait_set, const rmw_time_t * wait_timeout) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + wait set handle, + wait_set->implementation_identifier, rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + RCUTILS_LOG_DEBUG_NAMED( + "rmw_zenoh_cpp", + "[rmw_wait] %ld subscriptions, %ld services, %ld clients, %ld events, %ld guard conditions", + subscriptions->subscriber_count, + services->service_count, + clients->client_count, + events->event_count, + guard_conditions->guard_condition_count); + + if (wait_timeout) { + RCUTILS_LOG_DEBUG_NAMED( + "rmw_zenoh_common_cpp", "[rmw_wait] TIMEOUT: %ld s %ld ns", + wait_timeout->sec, + wait_timeout->nsec); + } + + auto wait_set_info = static_cast(wait_set->data); + if (!wait_set_info) { + RMW_SET_ERROR_MSG("Waitset info struct is null"); + return RMW_RET_ERROR; + } + + std::mutex * condition_mutex = &wait_set_info->condition_mutex; + if (!condition_mutex) { + RMW_SET_ERROR_MSG("Mutex for wait set was null"); + return RMW_RET_ERROR; + } + + std::condition_variable * condition_variable = &wait_set_info->condition; + if (!condition_variable) { + RMW_SET_ERROR_MSG("Condition variable for wait set was null"); + return RMW_RET_ERROR; + } + + std::unique_lock lock(*condition_mutex); + + bool ready = check_wait_conditions( + subscriptions, + guard_conditions, + services, + clients, + events, + false); + auto predicate = [subscriptions, guard_conditions, services, clients, events]() { + return check_wait_conditions( + subscriptions, + guard_conditions, + services, + clients, + events, + false); + }; + + bool timed_out = false; + + if (!ready) { + if (!wait_timeout) { + // TODO(CH3): Remove this magic number once stable. This is to slow things down so things are + // visible with all the printouts flying everywhere. + condition_variable->wait_for(lock, std::chrono::milliseconds(500), predicate); + } else if (wait_timeout->sec > 0 || wait_timeout->nsec > 0) { + auto wait_time = std::chrono::duration_cast( + std::chrono::seconds(wait_timeout->sec)); + wait_time += std::chrono::nanoseconds(wait_timeout->nsec); + + timed_out = !condition_variable->wait_for(lock, wait_time, predicate); + } else { + timed_out = true; + } + } + + // The finalize parameter passed in here enables debug and setting of non-ready conditions + // to NULL (as expected by rcl) + // + // (In other words, it ensures that this happens once per rmw_wait call) + // + // Debug logs and NULL assignments do not happen in the predicate above, and only on this call + check_wait_conditions(subscriptions, guard_conditions, services, clients, events, true); + lock.unlock(); + + if (timed_out) { + RCUTILS_LOG_DEBUG_NAMED("rmw_zenoh_common_cpp", "[rmw_wait] TIMED OUT"); + return RMW_RET_TIMEOUT; + } else { + return RMW_RET_OK; + } } //============================================================================== From f1a26b525fbc6b45955ea98aabe4af02458688ab Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sun, 10 Sep 2023 22:39:49 +0800 Subject: [PATCH 20/50] Update readme Signed-off-by: Yadunund --- README.md | 42 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index f8e2ad93..1b0dd236 100644 --- a/README.md +++ b/README.md @@ -1 +1,41 @@ -# rmw_alternative \ No newline at end of file +# rmw_alternative + +A ROS 2 RMW implementation based on Zenoh that is written using the zenoh-c bindings. + +## Requirements +- [ROS 2](https://docs.ros.org): Rolling/Iron/Humble + + +## Setup + +Install latest rustc. +> Note: The version of rustc that can be installed via apt is outdated. +```bash +curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh +``` + +Build `rmw_zenoh_cpp` + +```bash +mkdir ~/ws_rmw_alternative/src -p && cd ~/ws_rmw_alternative/src +git clone git@github.com:yadunund/rmw_alternative +cd ~/ws_rmw_alternative +source /opt/ros//setup.bash # replace with ROS 2 distro of choice +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release + +``` + +## Test +```bash +cd ~/ws_rmw_alternative +source install/setup.bash +export RMW_IMPLEMENTATION=rmw_zenoh_cpp +ros2 topic pub "/chatter" std_msgs/msg/String '{data: hello}' +``` + +## TODO Features +- [x] Publisher +- [ ] Subscription +- [ ] Client +- [ ] Service +- [ ] Graph introspection From 252d30ab4438f6e14542423a0173378f12380922 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 12 Sep 2023 10:16:26 +0800 Subject: [PATCH 21/50] Switch to master Signed-off-by: Yadunund --- zenoh_c_vendor/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 4f937a57..93de0a07 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(ament_cmake_vendor_package REQUIRED) ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git - VCS_VERSION 0.10.0-dev + VCS_VERSION master ) # set(INSTALL_DIR "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-prefix/install") From 817a22acf7ed750a50367bb7447731470b52c452 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 12 Sep 2023 23:38:14 +0800 Subject: [PATCH 22/50] interm service implementation needed as nodes now spin type_introspection services Signed-off-by: Yadunund --- README.md | 2 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 60 +++++++++++++++++++++++++++++---- 2 files changed, 55 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 1b0dd236..c99c60ec 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ A ROS 2 RMW implementation based on Zenoh that is written using the zenoh-c bindings. ## Requirements -- [ROS 2](https://docs.ros.org): Rolling/Iron/Humble +- [ROS 2](https://docs.ros.org): Rolling/Iron ## Setup diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index c9d944ac..a135fc3b 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -919,11 +919,51 @@ rmw_client_response_subscription_get_actual_qos( rmw_service_t * rmw_create_service( const rmw_node_t * node, - const rosidl_service_type_support_t * type_support, + const rosidl_service_type_support_t * type_supports, const char * service_name, - const rmw_qos_profile_t * qos_profile) + const rmw_qos_profile_t * qos_profiles) { - return nullptr; + // Interim implementation to suppress type_description service that spins up + // with a node by default. + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + rmw_zenoh_identifier, + return nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr); + if (0 == strlen(service_name)) { + RMW_SET_ERROR_MSG("service_name argument is an empty string"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_profiles, nullptr); + if (!qos_profiles->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(service_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("service_name argument is invalid: %s", reason); + return nullptr; + } + } + // rmw_qos_profile_t adapted_qos_policies = + // rmw_dds_common::qos_profile_update_best_available_for_services(*qos_policies); + // if (!is_valid_qos(adapted_qos_policies)) { + // RMW_SET_ERROR_MSG("create_service() called with invalid QoS"); + // return nullptr; + // } + + rmw_service_t * service = rmw_service_allocate(); + if (!service) { + RMW_SET_ERROR_MSG("failed to allocate rmw_service_t"); + return nullptr; + } + + return service; } //============================================================================== @@ -931,7 +971,13 @@ rmw_create_service( rmw_ret_t rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) { - return RMW_RET_UNSUPPORTED; + // Interim implementation to suppress type_description service that spins up + // with a node by default. + if (node == nullptr || service == nullptr) { + return RMW_RET_ERROR; + } + rmw_service_free(service); + return RMW_RET_OK; } //============================================================================== @@ -964,7 +1010,8 @@ rmw_service_request_subscription_get_actual_qos( const rmw_service_t * service, rmw_qos_profile_t * qos) { - return RMW_RET_UNSUPPORTED; + //TODO(yadunund): Fix. + return RMW_RET_OK; } //============================================================================== @@ -974,7 +1021,8 @@ rmw_service_response_publisher_get_actual_qos( const rmw_service_t * service, rmw_qos_profile_t * qos) { - return RMW_RET_UNSUPPORTED; + //TODO(yadunund): Fix. + return RMW_RET_OK; } //============================================================================== From bfc77e7109d5df2d123ae8d70260a2ea14956bdc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 19 Sep 2023 23:59:49 -0400 Subject: [PATCH 23/50] Use modern CMake targets for linking. (#4) Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index b6a00b5d..6e96da22 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -29,8 +29,6 @@ find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(zenoh_c_vendor REQUIRED) find_package(zenohc REQUIRED) - - add_library(rmw_zenoh_cpp SHARED src/detail/identifier.cpp src/detail/guard_condition.cpp @@ -55,9 +53,8 @@ target_link_libraries(rmw_zenoh_cpp rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp rmw::rmw - ${rmw_dds_common_TARGETS} - ${rmw_fastrtps_shared_cpp_TARGETS} - ${rmw_fastrtps_shared_cpp_LIBRARIES} + rmw_dds_common::rmw_dds_common_library + rmw_fastrtps_shared_cpp::rmw_fastrtps_shared_cpp zenohc::lib ) From 34638208eab15221385518105fcc7f5b900b93af Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 20 Sep 2023 14:22:42 +0800 Subject: [PATCH 24/50] Add build and style ci (#7) Signed-off-by: Yadunund --- .github/workflows/build.yaml | 30 ++++++++++++++++++++++++++++++ .github/workflows/style.yaml | 21 +++++++++++++++++++++ README.md | 3 +++ 3 files changed, 54 insertions(+) create mode 100644 .github/workflows/build.yaml create mode 100644 .github/workflows/style.yaml diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml new file mode 100644 index 00000000..14bc3616 --- /dev/null +++ b/.github/workflows/build.yaml @@ -0,0 +1,30 @@ +name: build +on: + pull_request: + push: + branches: [ main ] +defaults: + run: + shell: bash +jobs: + test: + runs-on: ubuntu-latest + strategy: + matrix: + docker_image: ['ros:iron-ros-base', 'ros:rolling-ros-base'] + container: + image: ${{ matrix.docker_image }} + timeout-minutes: 30 + steps: + - name: Deps + run: | + apt update && apt install -y curl + - name: Setup Rust + uses: dtolnay/rust-toolchain@stable + - uses: actions/checkout@v2 + - name: rosdep + run: | + rosdep update + rosdep install --from-paths . -yir + - name: build + run: /ros_entrypoint.sh colcon build diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml new file mode 100644 index 00000000..a2a0cb33 --- /dev/null +++ b/.github/workflows/style.yaml @@ -0,0 +1,21 @@ +name: style +on: + pull_request: + push: + branches: [ main ] +defaults: + run: + shell: bash +jobs: + test: + runs-on: ubuntu-latest + strategy: + matrix: + docker_image: ['ros:iron-ros-base', 'ros:rolling-ros-base'] + container: + image: ${{ matrix.docker_image }} + timeout-minutes: 30 + steps: + - uses: actions/checkout@v2 + - name: style + run: /ros_entrypoint.sh ament_uncrustify rmw_zenoh_cpp/ diff --git a/README.md b/README.md index c99c60ec..431ef9a2 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,8 @@ # rmw_alternative +![](https://github.com/yadunund/rmw_alternative/workflows/build/badge.svg) +![](https://github.com/yadunund/rmw_alternative/workflows/style/badge.svg) + A ROS 2 RMW implementation based on Zenoh that is written using the zenoh-c bindings. ## Requirements From abe42982cd41027885c6bc89005ae8593f670180 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 20 Sep 2023 02:28:17 -0400 Subject: [PATCH 25/50] Fix up leaks in the node and publisher initialization (#5) * Make sure to cleanup in both context init and fini. Signed-off-by: Chris Lalancette * Fixup style in the GuardCondition class. Signed-off-by: Chris Lalancette * Fix up leaks in rmw_publish family of functions. Signed-off-by: Chris Lalancette --------- Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/src/detail/guard_condition.cpp | 48 ++++++++++---------- rmw_zenoh_cpp/src/detail/guard_condition.hpp | 25 +++++----- rmw_zenoh_cpp/src/rmw_init.cpp | 28 +++++++----- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 39 ++++++++-------- 4 files changed, 73 insertions(+), 67 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.cpp b/rmw_zenoh_cpp/src/detail/guard_condition.cpp index d5f17139..7ce3f0b3 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.cpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.cpp @@ -17,56 +17,56 @@ ///============================================================================== GuardCondition::GuardCondition() -: hasTriggered_(false), - conditionMutex_(nullptr), - conditionVariable_(nullptr) {} +: has_triggered_(false), + condition_mutex_(nullptr), + condition_variable_(nullptr) {} ///============================================================================== void GuardCondition::trigger() { - std::lock_guard lock(internalMutex_); + std::lock_guard lock(internal_mutex_); - if (conditionMutex_ != nullptr) { - std::unique_lock clock(*conditionMutex_); + if (condition_mutex_ != nullptr) { + std::unique_lock clock(*condition_mutex_); // the change to hasTriggered_ needs to be mutually exclusive with // rmw_wait() which checks hasTriggered() and decides if wait() needs to // be called - hasTriggered_ = true; + has_triggered_ = true; clock.unlock(); - conditionVariable_->notify_one(); + condition_variable_->notify_one(); } else { - hasTriggered_ = true; + has_triggered_ = true; } } ///============================================================================== -void GuardCondition::attachCondition( - std::mutex * conditionMutex, - std::condition_variable * conditionVariable) +void GuardCondition::attach_condition( + std::mutex * condition_mutex, + std::condition_variable * condition_variable) { - std::lock_guard lock(internalMutex_); - conditionMutex_ = conditionMutex; - conditionVariable_ = conditionVariable; + std::lock_guard lock(internal_mutex_); + condition_mutex_ = condition_mutex; + condition_variable_ = condition_variable; } ///============================================================================== -void GuardCondition::detachCondition() +void GuardCondition::detach_condition() { - std::lock_guard lock(internalMutex_); - conditionMutex_ = nullptr; - conditionVariable_ = nullptr; + std::lock_guard lock(internal_mutex_); + condition_mutex_ = nullptr; + condition_variable_ = nullptr; } ///============================================================================== -bool GuardCondition::hasTriggered() +bool GuardCondition::has_triggered() const { - return hasTriggered_; + return has_triggered_; } ///============================================================================== -bool GuardCondition::getHasTriggered() +bool GuardCondition::get_has_triggered() { - bool ret = hasTriggered_; - hasTriggered_ = false; + bool ret = has_triggered_; + has_triggered_ = false; return ret; } diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.hpp b/rmw_zenoh_cpp/src/detail/guard_condition.hpp index 5af51f84..8f06ea96 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.hpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.hpp @@ -16,36 +16,33 @@ #ifndef SRC__DETAIL__GUARDCONDITION_HPP #define SRC__DETAIL__GUARDCONDITION_HPP -#include #include -#include #include #include -#include ///============================================================================== -class GuardCondition +class GuardCondition final { public: GuardCondition(); void trigger(); - void attachCondition( - std::mutex * conditionMutex, - std::condition_variable * conditionVariable); + void attach_condition( + std::mutex * condition_mutex, + std::condition_variable * condition_variable); - void detachCondition(); + void detach_condition(); - bool hasTriggered(); + bool has_triggered() const; - bool getHasTriggered(); + bool get_has_triggered(); private: - std::mutex internalMutex_; - std::atomic_bool hasTriggered_; - std::mutex * conditionMutex_; - std::condition_variable * conditionVariable_; + std::mutex internal_mutex_; + std::atomic_bool has_triggered_; + std::mutex * condition_mutex_; + std::condition_variable * condition_variable_; }; #endif // SRC__DETAIL__GUARDCONDITION_HPP diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 4ac3b2e3..c2eb492c 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -77,6 +77,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) if ((ret = rmw_init_options_copy(options, &context->options)) != RMW_RET_OK) { return ret; } + auto free_options = rcpputils::make_scope_exit([context]() {rmw_ret_t ret = rmw_init_options_fini(&context->options); (void)ret;}); // Initialize context's implementation context->impl->is_shutdown = false; @@ -107,21 +108,21 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) } // Initialize the guard condition. - // context->impl->graph_guard_condition = static_cast( - // option->allocator.allocate(sizeof(rmw_guard_condition_t), options->allocator.state)); - context->impl->graph_guard_condition = rmw_guard_condition_allocate(); - context->impl->graph_guard_condition->implementation_identifier = rmw_zenoh_identifier; - context->impl->graph_guard_condition->data = new GuardCondition(); - - if (!context->impl->graph_guard_condition) { - if (context->impl->graph_guard_condition->data) { - delete static_cast(context->impl->graph_guard_condition->data); - } - rmw_guard_condition_free(context->impl->graph_guard_condition); + RMW_SET_ERROR_MSG("Error allocating memory for guard condition"); + return RMW_RET_BAD_ALLOC; + } + auto cleanup_guard_condition = rcpputils::make_scope_exit([context]() {rmw_guard_condition_free(context->impl->graph_guard_condition);}); + context->impl->graph_guard_condition->implementation_identifier = rmw_zenoh_identifier; + context->impl->graph_guard_condition->data = new (std::nothrow) GuardCondition(); + if (!context->impl->graph_guard_condition->data) { + RMW_SET_ERROR_MSG("Error allocating memory for guard condition data"); + return RMW_RET_BAD_ALLOC; } + cleanup_guard_condition.cancel(); + free_options.cancel(); cleanup_impl.cancel(); restore_context.cancel(); return RMW_RET_OK; @@ -171,6 +172,11 @@ rmw_context_fini(rmw_context_t * context) RCUTILS_SET_ERROR_MSG("context has not been shutdown"); return RMW_RET_INVALID_ARGUMENT; } + + delete static_cast(context->impl->graph_guard_condition->data); + + rmw_guard_condition_free(context->impl->graph_guard_condition); + rmw_ret_t ret = rmw_init_options_fini(&context->options); delete context->impl; *context = rmw_get_zero_initialized_context(); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index a135fc3b..cf5a340f 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -181,7 +181,7 @@ rmw_node_get_graph_guard_condition(const rmw_node_t * node) node->implementation_identifier, rmw_zenoh_identifier, return nullptr); - // TODO(Yadunund): Also check if node->data is valud. + // TODO(Yadunund): Also check if node->data is valid. return node->context->impl->graph_guard_condition; } @@ -229,7 +229,7 @@ rmw_create_publisher( return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); - if (0 == strlen(topic_name)) { + if (topic_name[0] == '\0') { RMW_SET_ERROR_MSG("topic_name argument is an empty string"); return nullptr; } @@ -310,15 +310,15 @@ rmw_create_publisher( // RMW_SET_ERROR_MSG("unable to cast publisher data into rmw_publisher_data_t"); // return nullptr; // } + auto free_rmw_publisher = rcpputils::make_scope_exit([rmw_publisher](){rmw_publisher_free(rmw_publisher);}); rcutils_allocator_t * allocator = &node->context->options.allocator; auto publisher_data = static_cast( allocator->allocate(sizeof(rmw_publisher_data_t), allocator->state)); if (publisher_data == nullptr) { RMW_SET_ERROR_MSG("failed to allocate publisher data"); - allocator->deallocate(publisher_data, allocator->state); - allocator->deallocate(rmw_publisher, allocator->state); return nullptr; } + auto free_publisher_data = rcpputils::make_scope_exit([publisher_data, allocator](){allocator->deallocate(publisher_data, allocator->state);}); // TODO(yadunund): Zenoh key cannot contain leading or trailing '/' so we strip them. // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate @@ -332,35 +332,33 @@ rmw_create_publisher( RMW_SET_ERROR_MSG("unable to create publisher"); return nullptr; } + auto undeclare_z_publisher = rcpputils::make_scope_exit([publisher_data](){z_undeclare_publisher(z_move(publisher_data->pub));}); + publisher_data->typesupport_identifier = type_support->typesupport_identifier; publisher_data->type_support_impl = type_support->data; auto callbacks = static_cast(type_support->data); publisher_data->type_support = static_cast( allocator->allocate(sizeof(MessageTypeSupport), allocator->state)); - new(publisher_data->type_support) MessageTypeSupport(callbacks); RMW_CHECK_FOR_NULL_WITH_MSG( publisher_data->type_support, "Failed to allocate MessageTypeSupport", return nullptr); + auto free_type_support = rcpputils::make_scope_exit([publisher_data, allocator](){allocator->deallocate(publisher_data->type_support, allocator->state);}); + + new(publisher_data->type_support) MessageTypeSupport(callbacks); publisher_data->context = node->context; rmw_publisher->data = publisher_data; - auto cleanup_rmw_publisher = rcpputils::make_scope_exit( - [rmw_publisher]() { - auto publisher_data = static_cast(rmw_publisher->data); - z_undeclare_publisher(z_move(publisher_data->pub)); - rmw_free(const_cast(rmw_publisher->topic_name)); - rmw_publisher_free(rmw_publisher); - }); - rmw_publisher->implementation_identifier = rmw_zenoh_identifier; - rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - RMW_CHECK_ARGUMENT_FOR_NULL(rmw_publisher->topic_name, nullptr); - memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); rmw_publisher->options = *publisher_options; // TODO(yadunund): Update this. rmw_publisher->can_loan_messages = false; + rmw_publisher->topic_name = rcutils_strdup(topic_name, *allocator); + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_publisher->topic_name, nullptr); - cleanup_rmw_publisher.cancel(); + free_type_support.cancel(); + undeclare_z_publisher.cancel(); + free_publisher_data.cancel(); + free_rmw_publisher.cancel(); return rmw_publisher; } @@ -383,14 +381,19 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); rmw_ret_t ret = RMW_RET_OK; + + rmw_free(const_cast(publisher->topic_name)); + auto publisher_data = static_cast(publisher->data); if (publisher_data != nullptr) { + rcutils_allocator_t * allocator = &node->context->options.allocator; + allocator->deallocate(publisher_data->type_support, allocator->state); if (z_undeclare_publisher(z_move(publisher_data->pub))) { RMW_SET_ERROR_MSG("failed to undeclare pub"); ret = RMW_RET_ERROR; } + allocator->deallocate(publisher_data, allocator->state); } - rmw_free(const_cast(publisher->topic_name)); rmw_publisher_free(publisher); return ret; } From abc1cba1a0d82d0cb5fa492685df4e1044329362 Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 20 Sep 2023 23:57:39 +0800 Subject: [PATCH 26/50] uncrustify (#11) Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/rmw_init.cpp | 10 ++++++++-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 20 ++++++++++++++++---- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index c2eb492c..8dcc5891 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -77,7 +77,10 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) if ((ret = rmw_init_options_copy(options, &context->options)) != RMW_RET_OK) { return ret; } - auto free_options = rcpputils::make_scope_exit([context]() {rmw_ret_t ret = rmw_init_options_fini(&context->options); (void)ret;}); + auto free_options = rcpputils::make_scope_exit( + [context]() { + rmw_ret_t ret = rmw_init_options_fini(&context->options); (void)ret; + }); // Initialize context's implementation context->impl->is_shutdown = false; @@ -113,7 +116,10 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) RMW_SET_ERROR_MSG("Error allocating memory for guard condition"); return RMW_RET_BAD_ALLOC; } - auto cleanup_guard_condition = rcpputils::make_scope_exit([context]() {rmw_guard_condition_free(context->impl->graph_guard_condition);}); + auto cleanup_guard_condition = rcpputils::make_scope_exit( + [context]() { + rmw_guard_condition_free(context->impl->graph_guard_condition); + }); context->impl->graph_guard_condition->implementation_identifier = rmw_zenoh_identifier; context->impl->graph_guard_condition->data = new (std::nothrow) GuardCondition(); if (!context->impl->graph_guard_condition->data) { diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index cf5a340f..26fee5b9 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -310,7 +310,10 @@ rmw_create_publisher( // RMW_SET_ERROR_MSG("unable to cast publisher data into rmw_publisher_data_t"); // return nullptr; // } - auto free_rmw_publisher = rcpputils::make_scope_exit([rmw_publisher](){rmw_publisher_free(rmw_publisher);}); + auto free_rmw_publisher = rcpputils::make_scope_exit( + [rmw_publisher]() { + rmw_publisher_free(rmw_publisher); + }); rcutils_allocator_t * allocator = &node->context->options.allocator; auto publisher_data = static_cast( allocator->allocate(sizeof(rmw_publisher_data_t), allocator->state)); @@ -318,7 +321,10 @@ rmw_create_publisher( RMW_SET_ERROR_MSG("failed to allocate publisher data"); return nullptr; } - auto free_publisher_data = rcpputils::make_scope_exit([publisher_data, allocator](){allocator->deallocate(publisher_data, allocator->state);}); + auto free_publisher_data = rcpputils::make_scope_exit( + [publisher_data, allocator]() { + allocator->deallocate(publisher_data, allocator->state); + }); // TODO(yadunund): Zenoh key cannot contain leading or trailing '/' so we strip them. // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate @@ -332,7 +338,10 @@ rmw_create_publisher( RMW_SET_ERROR_MSG("unable to create publisher"); return nullptr; } - auto undeclare_z_publisher = rcpputils::make_scope_exit([publisher_data](){z_undeclare_publisher(z_move(publisher_data->pub));}); + auto undeclare_z_publisher = rcpputils::make_scope_exit( + [publisher_data]() { + z_undeclare_publisher(z_move(publisher_data->pub)); + }); publisher_data->typesupport_identifier = type_support->typesupport_identifier; publisher_data->type_support_impl = type_support->data; @@ -343,7 +352,10 @@ rmw_create_publisher( publisher_data->type_support, "Failed to allocate MessageTypeSupport", return nullptr); - auto free_type_support = rcpputils::make_scope_exit([publisher_data, allocator](){allocator->deallocate(publisher_data->type_support, allocator->state);}); + auto free_type_support = rcpputils::make_scope_exit( + [publisher_data, allocator]() { + allocator->deallocate(publisher_data->type_support, allocator->state); + }); new(publisher_data->type_support) MessageTypeSupport(callbacks); publisher_data->context = node->context; From b28eca6b2c7eda3beaf540ddeaaecd70951f60aa Mon Sep 17 00:00:00 2001 From: Yadu Date: Thu, 21 Sep 2023 00:00:04 +0800 Subject: [PATCH 27/50] Skeleton for subscription (#8) * Implement create_subscription Signed-off-by: Yadunund * Store messages in queue Signed-off-by: Yadunund * Implement rmw_take_with_info Signed-off-by: Yadunund * Fix memory leak in subscription Signed-off-by: Yadunund * Specify encoding for publishers Signed-off-by: Yadunund * Add node on encoding Signed-off-by: Yadunund * complete check_wait_conditions Signed-off-by: Yadunund --------- Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 1 + rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp | 35 -- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 59 +++ rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 46 +++ .../src/rmw_get_topic_endpoint_info.cpp | 4 +- rmw_zenoh_cpp/src/rmw_init.cpp | 2 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 382 +++++++++++++++++- 7 files changed, 477 insertions(+), 52 deletions(-) delete mode 100644 rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp create mode 100644 rmw_zenoh_cpp/src/detail/rmw_data_types.cpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 6e96da22..e8d381ee 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(zenohc REQUIRED) add_library(rmw_zenoh_cpp SHARED src/detail/identifier.cpp src/detail/guard_condition.cpp + src/detail/rmw_data_types.cpp src/detail/type_support_common.cpp src/rmw_event.cpp src/rmw_get_network_flow_endpoints.cpp diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp deleted file mode 100644 index 4f2d1d78..00000000 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2023 Open Source Robotics Foundation, Inc. -// -// 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 SRC__DETAIL__RMW_CONTEXT_IMPL_HPP -#define SRC__DETAIL__RMW_CONTEXT_IMPL_HPP - -#include "rmw/rmw.h" - -#include "zenoh.h" - -struct rmw_context_impl_s -{ - // An owned session. - z_owned_session_t session; - - /// Shutdown flag. - bool is_shutdown; - - // Equivalent to rmw_dds_common::Context's guard condition - /// Guard condition that should be triggered when the graph changes. - rmw_guard_condition_t * graph_guard_condition; -}; - -#endif // SRC__DETAIL__RMW_CONTEXT_IMPL_HPP diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp new file mode 100644 index 00000000..33423904 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -0,0 +1,59 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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_data_types.hpp" + +std::mutex sub_callback_mutex; + +///============================================================================== +void sub_data_handler( + const z_sample_t * sample, + void * data) +{ + z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); + // TODO(yadunund): Remove after debugging. + printf( + ">> [Subscriber] Received ('%s': size: '%i', payload: '%s')\n", z_loan(keystr), + (int)sample->payload.len, sample->payload.start); + + auto sub_data = static_cast(data); + if (sub_data == nullptr) { + return; + } + + std::lock_guard guard(sub_callback_mutex); + // Vector to store the byte array (so we have a copyable container instead of a pointer) + std::vector byte_vec(sample->payload.start, + sample->payload.start + sample->payload.len); + auto byte_vec_ptr = std::make_shared>(std::move(byte_vec)); + + std::unique_lock lock(sub_data->message_queue_mutex); + + if (sub_data->message_queue.size() >= sub_data->queue_depth) { + // Log warning if message is discarded due to hitting the queue depth + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "Message queue depth of %ld reached, discarding oldest message " + "for subscription for %s", + sub_data->queue_depth, + z_loan(keystr)); + + sub_data->message_queue.pop_back(); + } + sub_data->message_queue.push_front(byte_vec_ptr); + + sub_data->message_queue_mutex.unlock(); + + z_drop(z_move(keystr)); +} diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index d72d3085..2acde1e0 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rmw/rmw.h" @@ -25,6 +26,31 @@ #include "zenoh.h" /// Structs for various type erased data fields. + +///============================================================================== +struct rmw_context_impl_s +{ + // An owned session. + z_owned_session_t session; + + /// Shutdown flag. + bool is_shutdown; + + // Equivalent to rmw_dds_common::Context's guard condition + /// Guard condition that should be triggered when the graph changes. + rmw_guard_condition_t * graph_guard_condition; +}; + +///============================================================================== +struct rmw_node_data_t +{ + // TODO(yadunund): Add a GraphCache object. + + // Map topic name to topic types. + std::unordered_set> publishers; + std::unordered_set> subscriptions; +}; + ///============================================================================== struct rmw_publisher_data_t { @@ -47,4 +73,24 @@ struct rmw_wait_set_data_t std::mutex condition_mutex; }; +///============================================================================== +// z_owned_closure_sample_t +void sub_data_handler(const z_sample_t * sample, void * sub_data); + +///============================================================================== +struct rmw_subscription_data_t +{ + z_owned_subscriber_t sub; + + const void * type_support_impl; + const char * typesupport_identifier; + TypeSupport * type_support; + + std::deque>> message_queue; + std::mutex message_queue_mutex; + + size_t queue_depth; + bool reliable; +}; + #endif // SRC__DETAIL__RMW_DATA_TYPES_HPP diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp index aa1169b4..298602b0 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -27,7 +27,7 @@ rmw_get_publishers_info_by_topic( bool no_mangle, rmw_topic_endpoint_info_array_t * publishers_info) { - return RMW_RET_UNSUPPORTED; + return RMW_RET_OK; } ///============================================================================== @@ -40,7 +40,7 @@ rmw_get_subscriptions_info_by_topic( bool no_mangle, rmw_topic_endpoint_info_array_t * subscriptions_info) { - return RMW_RET_UNSUPPORTED; + return RMW_RET_OK; } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 8dcc5891..90be17a9 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -14,7 +14,7 @@ #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" -#include "detail/rmw_context_impl.hpp" +#include "detail/rmw_data_types.hpp" #include "rcutils/env.h" diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 26fee5b9..9a6796f9 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -14,7 +14,6 @@ #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" -#include "detail/rmw_context_impl.hpp" #include "detail/rmw_data_types.hpp" #include "detail/serialization_format.hpp" #include "detail/type_support_common.hpp" @@ -125,6 +124,7 @@ rmw_create_node( [node]() { rmw_free(const_cast(node->name)); rmw_free(const_cast(node->namespace_)); + rmw_free(static_cast(node->data)); rmw_node_free(node); }); @@ -140,6 +140,12 @@ rmw_create_node( // TODO(yadunund): Register with storage system here and throw error if // zenohd is not running. // Put metadata into node->data. + auto node_data = static_cast(rmw_allocate(sizeof(rmw_node_data_t))); + if (node_data == nullptr) { + RMW_SET_ERROR_MSG("failed to allocate node data"); + return nullptr; + } + node->data = node_data; cleanup_node.cancel(); node->implementation_identifier = rmw_zenoh_identifier; @@ -165,6 +171,7 @@ rmw_destroy_node(rmw_node_t * node) // rmw_context_t * context = node->context; rmw_free(const_cast(node->name)); rmw_free(const_cast(node->namespace_)); + rmw_free(static_cast(node->data)); rmw_node_free(const_cast(node)); // delete node_impl; return result_ret; @@ -335,7 +342,7 @@ rmw_create_publisher( NULL ); if (!z_check(publisher_data->pub)) { - RMW_SET_ERROR_MSG("unable to create publisher"); + RMW_SET_ERROR_MSG("unable to create zenoh publisher"); return nullptr; } auto undeclare_z_publisher = rcpputils::make_scope_exit( @@ -527,12 +534,17 @@ rmw_publish( const size_t data_length = ser.getSerializedDataLength(); + // The encoding is simply forwarded and is useful when key expressions in the + // session use different encoding formats. In our case, all key expressions + // will be encoded with CDR so it does not really matter. + z_publisher_put_options_t options = z_publisher_put_options_default(); + options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); // Returns 0 if success. int8_t ret = z_publisher_put( z_loan(publisher_data->pub), (const uint8_t *)msg_bytes, data_length, - NULL); + &options); allocator->deallocate(msg_bytes, allocator->state); @@ -619,12 +631,17 @@ rmw_publish_serialized_message( const size_t data_length = ser.getSerializedDataLength(); + // The encoding is simply forwarded and is useful when key expressions in the + // session use different encoding formats. In our case, all key expressions + // will be encoded with CDR so it does not really matter. + z_publisher_put_options_t options = z_publisher_put_options_default(); + options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); // Returns 0 if success. int8_t ret = z_publisher_put( z_loan(publisher_data->pub), serialized_message->buffer, data_length, - NULL); + &options); if (ret) { RMW_SET_ERROR_MSG("unable to publish message"); @@ -693,6 +710,10 @@ rmw_init_subscription_allocation( const rosidl_runtime_c__Sequence__bound * message_bounds, rmw_subscription_allocation_t * allocation) { + // Unused in current implementation. + (void) type_support; + (void) message_bounds; + (void) allocation; return RMW_RET_UNSUPPORTED; } @@ -702,6 +723,7 @@ rmw_ret_t rmw_fini_subscription_allocation( rmw_subscription_allocation_t * allocation) { + (void) allocation; return RMW_RET_UNSUPPORTED; } @@ -710,12 +732,167 @@ rmw_fini_subscription_allocation( rmw_subscription_t * rmw_create_subscription( const rmw_node_t * node, - const rosidl_message_type_support_t * type_support, + const rosidl_message_type_support_t * type_supports, const char * topic_name, const rmw_qos_profile_t * qos_profile, const rmw_subscription_options_t * subscription_options) { - return nullptr; + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + rmw_zenoh_identifier, + return nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) { + RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_profile, nullptr); + // Adapt any 'best available' QoS options + // rmw_qos_profile_t adapted_qos_profile = *qos_profile; + // rmw_ret_t ret = rmw_dds_common::qos_profile_get_best_available_for_topic_subscription( + // node, topic_name, &adapted_qos_profile, rmw_get_publishers_info_by_topic); + // if (RMW_RET_OK != ret) { + // return nullptr; + // } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + + // Check RMW QoS + // if (!is_valid_qos(*qos_profile)) { + // RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + // return nullptr; + // } + + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } + } + auto node_data = static_cast(node->data); + RMW_CHECK_FOR_NULL_WITH_MSG( + node_data, "unable to create subscription as node_data is invalid.", + return nullptr); + // TODO(yadunund): Check if a duplicate entry for the same topic name + topic type + // is present in node_data->subscriptions and if so return error; + RMW_CHECK_FOR_NULL_WITH_MSG( + node->context, + "expected initialized context", + return nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG( + node->context->impl, + "expected initialized context impl", + return nullptr); + rmw_context_impl_s * context_impl = static_cast( + node->context->impl); + RMW_CHECK_FOR_NULL_WITH_MSG( + context_impl, + "unable to get rmw_context_impl_s", + return nullptr); + if (!z_check(context_impl->session)) { + RMW_SET_ERROR_MSG("zenoh session is invalid"); + return nullptr; + } + + // Create the rmw_subscription. + rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); + return nullptr; + } + auto free_rmw_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() { + rmw_subscription_free(rmw_subscription); + }); + rcutils_allocator_t * allocator = &node->context->options.allocator; + auto * sub_data = + static_cast(allocator->allocate( + sizeof(rmw_subscription_data_t), + allocator->state)); + if (sub_data == nullptr) { + RMW_SET_ERROR_MSG("failed to allocate sub_data"); + return nullptr; + } + auto free_sub_data = rcpputils::make_scope_exit( + [sub_data, allocator]() { + allocator->deallocate(sub_data, allocator->state); + }); + + // Set the reliability of the subscription options based on qos_profile. + // The default options will be initialized with Best Effort reliability. + auto sub_options = z_subscriber_options_default(); + sub_data->reliable = false; + if (qos_profile->reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { + sub_options.reliability = Z_RELIABILITY_RELIABLE; + sub_data->reliable = true; + } + + z_owned_closure_sample_t callback = z_closure(sub_data_handler, nullptr, sub_data); + sub_data->sub = z_declare_subscriber( + z_loan(context_impl->session), + z_keyexpr(&topic_name[1]), + z_move(callback), + &sub_options + ); + if (!z_check(sub_data->sub)) { + RMW_SET_ERROR_MSG("unable to create zenoh subscription"); + return nullptr; + } + auto undeclare_z_sub = rcpputils::make_scope_exit( + [sub_data]() { + z_undeclare_subscriber(z_move(sub_data->sub)); + }); + + sub_data->type_support_impl = type_support->data; + sub_data->typesupport_identifier = type_support->typesupport_identifier; + auto callbacks = static_cast(type_support->data); + // std::string type_name = _create_type_name(callbacks); + sub_data->type_support = static_cast( + rmw_allocate(sizeof(MessageTypeSupport))); + RMW_CHECK_FOR_NULL_WITH_MSG( + sub_data->type_support, + "Failed to allocate MessageTypeSupport", + return nullptr); + auto free_type_support = rcpputils::make_scope_exit( + [sub_data, allocator]() { + allocator->deallocate(sub_data->type_support, allocator->state); + }); + + sub_data->queue_depth = qos_profile->depth; + new(sub_data->type_support) MessageTypeSupport(callbacks); + rmw_subscription->implementation_identifier = rmw_zenoh_identifier; + rmw_subscription->data = sub_data; + rmw_subscription->topic_name = rcutils_strdup(topic_name, *allocator); + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_subscription->topic_name, nullptr); + auto free_topic_name = rcpputils::make_scope_exit( + [rmw_subscription, allocator]() { + allocator->deallocate(const_cast(rmw_subscription->topic_name), allocator->state); + }); + rmw_subscription->options = *subscription_options; + rmw_subscription->can_loan_messages = false; + rmw_subscription->is_cft_enabled = false; + + free_topic_name.cancel(); + free_type_support.cancel(); + undeclare_z_sub.cancel(); + free_sub_data.cancel(); + free_rmw_subscription.cancel(); + return rmw_subscription; } //============================================================================== @@ -723,7 +900,35 @@ rmw_create_subscription( rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, + subscription->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + rmw_ret_t ret = RMW_RET_OK; + + rmw_free(const_cast(subscription->topic_name)); + + auto sub_data = static_cast(subscription->data); + if (sub_data != nullptr) { + rcutils_allocator_t * allocator = &node->context->options.allocator; + allocator->deallocate(sub_data->type_support, allocator->state); + if (z_undeclare_subscriber(z_move(sub_data->sub))) { + RMW_SET_ERROR_MSG("failed to undeclare sub"); + ret = RMW_RET_ERROR; + } + allocator->deallocate(sub_data, allocator->state); + } + rmw_subscription_free(subscription); + return ret; } //============================================================================== @@ -743,7 +948,20 @@ rmw_subscription_get_actual_qos( const rmw_subscription_t * subscription, rmw_qos_profile_t * qos) { - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, + subscription->implementation_identifier, + rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + auto sub_data = static_cast(subscription->data); + RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); + + qos->reliability = sub_data->reliable ? RMW_QOS_POLICY_RELIABILITY_RELIABLE : + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + return RMW_RET_OK; } //============================================================================== @@ -789,7 +1007,63 @@ rmw_take_with_info( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation) { - return RMW_RET_UNSUPPORTED; + + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription->topic_name, RMW_RET_ERROR); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription->data, RMW_RET_ERROR); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + (void) allocation; + *taken = false; + + auto * sub_data = static_cast(subscription->data); + + // RETRIEVE SERIALIZED MESSAGE =============================================== + std::unique_lock lock(sub_data->message_queue_mutex); + + if (sub_data->message_queue.empty()) { + // This tells rcl that the check for a new message was done, but no messages have come in yet. + return RMW_RET_OK; + } + + // NOTE(CH3): Potential place to handle "QoS" (e.g. could pop from back so it is LIFO) + auto msg_bytes_ptr = sub_data->message_queue.back(); + sub_data->message_queue.pop_back(); + sub_data->message_queue_mutex.unlock(); + + unsigned char * cdr_buffer = static_cast(rmw_allocate( + msg_bytes_ptr->size())); + memcpy(cdr_buffer, &msg_bytes_ptr->front(), msg_bytes_ptr->size()); + + // Object that manages the raw buffer + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(cdr_buffer), + msg_bytes_ptr->size()); + + // Object that serializes the data + eprosima::fastcdr::Cdr deser( + fastbuffer, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (!sub_data->type_support->deserializeROSmessage( + deser, + ros_message, + sub_data->type_support_impl)) + { + RMW_SET_ERROR_MSG("could not deserialize ROS message"); + return RMW_RET_ERROR; + } + + *taken = true; + rmw_free(cdr_buffer); + + return RMW_RET_OK; } //============================================================================== @@ -965,9 +1239,9 @@ rmw_create_service( return nullptr; } } - // rmw_qos_profile_t adapted_qos_policies = - // rmw_dds_common::qos_profile_update_best_available_for_services(*qos_policies); - // if (!is_valid_qos(adapted_qos_policies)) { + // rmw_qos_profile_t adapted_qos_profile = + // rmw_dds_common::qos_profile_update_best_available_for_services(*qos_profile); + // if (!is_valid_qos(adapted_qos_profile)) { // RMW_SET_ERROR_MSG("create_service() called with invalid QoS"); // return nullptr; // } @@ -1176,8 +1450,88 @@ bool check_wait_conditions( const rmw_events_t * events, bool finalize) { - // TODO(yadunund): Fixme. - return true; + // NOTE(CH3): On the finalize parameter + // This check function is used as a predicate to wait on a condition variable. But rcl expects + // rmw to set any passed in pointers to NULL if the condition is not ready. + // + // The finalize parameter is used to make sure that this setting to NULL only happens ONCE per + // rmw_wait call. Otherwise on repeat calls to check the predicate, things will break since + // it'll try to compare or dereference a nullptr. + + bool stop_wait = false; + + // SUBSCRIPTIONS ============================================================= + if (subscriptions) { + size_t subscriptions_ready = 0; + + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + auto subscription_data = static_cast( + subscriptions->subscribers[i]); + if (subscription_data && subscription_data->message_queue.empty()) { + if (finalize) { + // Setting to nullptr lets rcl know that this subscription is not ready + subscriptions->subscribers[i] = nullptr; + } + } else { + subscriptions_ready++; + stop_wait = true; + } + } + + if (finalize && subscriptions_ready > 0) { + RCUTILS_LOG_DEBUG_NAMED( + "rmw_zenoh_cpp", "[rmw_wait] SUBSCRIPTIONS READY: %ld", + subscriptions_ready); + } + } + + // SERVICES ================================================================== + // TODO(yadunund) + for (size_t i = 0; i < services->service_count; ++i) { + services->services[i] = nullptr; + } + + // CLIENTS =================================================================== + // TODO(yadunund) + for (size_t i = 0; i < clients->client_count; ++i) { + clients->clients[i] = nullptr; + } + + // Guard conditions + // for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + // guard_conditions->guard_conditions[i] = nullptr; + // } + if (guard_conditions) { + size_t guard_conditions_ready = 0; + + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + auto guard_condition = static_cast( + guard_conditions->guard_conditions[i]); + if (guard_condition && guard_condition->has_triggered()) { + if (finalize) { + // Setting to nullptr lets rcl know that this guard_condition is not ready + guard_conditions->guard_conditions[i] = nullptr; + } + } else { + guard_conditions_ready++; + stop_wait = true; + } + } + + if (finalize && guard_conditions_ready > 0) { + RCUTILS_LOG_DEBUG_NAMED( + "rmw_zenoh_common_cpp", "[rmw_wait] GUARD CONDITIONS READY: %ld", + guard_conditions_ready); + } + } + + // Events + // TODO(yadunund) + for (size_t i = 0; i < events->event_count; ++i) { + events->events[i] = nullptr; + } + + return stop_wait; } } // namespace anonymous From dd91fb1de5f870ff3c667d584f3e9bbb5673d735 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 21 Sep 2023 08:47:14 -0400 Subject: [PATCH 28/50] Enable linting on the repository. (#12) Also make the fixes necessary so that linting passes. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 4 ++++ rmw_zenoh_cpp/package.xml | 3 +++ rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp | 6 +++--- rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp | 6 +++--- rmw_zenoh_cpp/src/detail/TypeSupport.hpp | 6 +++--- rmw_zenoh_cpp/src/detail/guard_condition.hpp | 6 +++--- rmw_zenoh_cpp/src/detail/identifier.hpp | 6 +++--- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 6 +++++- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 14 +++++++++----- .../src/detail/rmw_init_options_impl.hpp | 8 ++++---- .../src/detail/serialization_format.hpp | 6 +++--- .../src/detail/type_support_common.hpp | 6 +++--- rmw_zenoh_cpp/src/rmw_event.cpp | 3 +-- .../src/rmw_get_network_flow_endpoints.cpp | 3 +-- .../src/rmw_get_node_info_and_types.cpp | 3 +-- .../src/rmw_get_service_names_and_types.cpp | 3 +-- .../src/rmw_get_topic_endpoint_info.cpp | 3 +-- .../src/rmw_get_topic_names_and_types.cpp | 3 +-- rmw_zenoh_cpp/src/rmw_init.cpp | 7 +++---- rmw_zenoh_cpp/src/rmw_init_options.cpp | 7 +++---- rmw_zenoh_cpp/src/rmw_qos.cpp | 2 -- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 17 ++++++++--------- 22 files changed, 66 insertions(+), 62 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index e8d381ee..a9e55ccf 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -74,6 +74,10 @@ register_rmw_implementation( "c:rosidl_typesupport_c:rosidl_typesupport_fastrtps_c:rosidl_typesupport_introspection_c" "cpp:rosidl_typesupport_cpp:rosidl_typesupport_fastrtps_cpp:rosidl_typesupport_introspection_cpp") +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() install( TARGETS rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index cb3c9be2..e68ecfd7 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -20,6 +20,9 @@ rmw_dds_common rmw_fastrtps_shared_cpp + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp b/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp index 97b5bc8e..5b2a810c 100644 --- a/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp +++ b/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp @@ -16,8 +16,8 @@ // This file is originally from: // https://github.com/ros2/rmw_fastrtps/blob/b13e134cea2852aba210299bef6f4df172d9a0e3/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp -#ifndef SRC__DETAIL__MESSAGETYPESUPPORT_HPP -#define SRC__DETAIL__MESSAGETYPESUPPORT_HPP +#ifndef DETAIL__MESSAGETYPESUPPORT_HPP_ +#define DETAIL__MESSAGETYPESUPPORT_HPP_ #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" #include "TypeSupport.hpp" @@ -29,4 +29,4 @@ class MessageTypeSupport : public TypeSupport explicit MessageTypeSupport(const message_type_support_callbacks_t * members); }; -#endif // SRC__DETAIL__MESSAGETYPESUPPORT_HPP +#endif // DETAIL__MESSAGETYPESUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp b/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp index 8134c908..2dc6291d 100644 --- a/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp +++ b/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp @@ -16,8 +16,8 @@ // This file is originally from: // https://github.com/ros2/rmw_fastrtps/blob/b13e134cea2852aba210299bef6f4df172d9a0e3/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp -#ifndef SRC__DETAIL__SERVICETYPESUPPORT_HPP -#define SRC__DETAIL__SERVICETYPESUPPORT_HPP +#ifndef DETAIL__SERVICETYPESUPPORT_HPP_ +#define DETAIL__SERVICETYPESUPPORT_HPP_ #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" #include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" @@ -42,4 +42,4 @@ class ResponseTypeSupport : public ServiceTypeSupport explicit ResponseTypeSupport(const service_type_support_callbacks_t * members); }; -#endif // SRC__DETAIL__SERVICETYPESUPPORT_HPP +#endif // DETAIL__SERVICETYPESUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/TypeSupport.hpp b/rmw_zenoh_cpp/src/detail/TypeSupport.hpp index 79082f24..3a9df353 100644 --- a/rmw_zenoh_cpp/src/detail/TypeSupport.hpp +++ b/rmw_zenoh_cpp/src/detail/TypeSupport.hpp @@ -16,8 +16,8 @@ // This file is originally from: // https://github.com/ros2/rmw_fastrtps/blob/b13e134cea2852aba210299bef6f4df172d9a0e3/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp -#ifndef SRC__DETAIL__TYPESUPPORT_HPP -#define SRC__DETAIL__TYPESUPPORT_HPP +#ifndef DETAIL__TYPESUPPORT_HPP_ +#define DETAIL__TYPESUPPORT_HPP_ // #include @@ -53,4 +53,4 @@ class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport bool has_data_; }; -#endif // SRC__DETAIL__TYPESUPPORT_HPP +#endif // DETAIL__TYPESUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.hpp b/rmw_zenoh_cpp/src/detail/guard_condition.hpp index 8f06ea96..a1c1b79c 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.hpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SRC__DETAIL__GUARDCONDITION_HPP -#define SRC__DETAIL__GUARDCONDITION_HPP +#ifndef DETAIL__GUARD_CONDITION_HPP_ +#define DETAIL__GUARD_CONDITION_HPP_ #include #include @@ -45,4 +45,4 @@ class GuardCondition final std::condition_variable * condition_variable_; }; -#endif // SRC__DETAIL__GUARDCONDITION_HPP +#endif // DETAIL__GUARD_CONDITION_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/identifier.hpp b/rmw_zenoh_cpp/src/detail/identifier.hpp index 752968b5..f7e73df9 100644 --- a/rmw_zenoh_cpp/src/detail/identifier.hpp +++ b/rmw_zenoh_cpp/src/detail/identifier.hpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SRC__DETAIL__IDENTIFIER_HPP -#define SRC__DETAIL__IDENTIFIER_HPP +#ifndef DETAIL__IDENTIFIER_HPP_ +#define DETAIL__IDENTIFIER_HPP_ extern const char * const rmw_zenoh_identifier; -#endif // SRC__DETAIL__IDENTIFIER_HPP +#endif // DETAIL__IDENTIFIER_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 33423904..eb9901c2 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include + #include "rmw_data_types.hpp" std::mutex sub_callback_mutex; @@ -25,7 +29,7 @@ void sub_data_handler( // TODO(yadunund): Remove after debugging. printf( ">> [Subscriber] Received ('%s': size: '%i', payload: '%s')\n", z_loan(keystr), - (int)sample->payload.len, sample->payload.start); + static_cast(sample->payload.len), sample->payload.start); auto sub_data = static_cast(data); if (sub_data == nullptr) { diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 2acde1e0..9cc2cbd4 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -12,19 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SRC__DETAIL__RMW_DATA_TYPES_HPP -#define SRC__DETAIL__RMW_DATA_TYPES_HPP +#ifndef DETAIL__RMW_DATA_TYPES_HPP_ +#define DETAIL__RMW_DATA_TYPES_HPP_ + +#include #include +#include +#include #include +#include #include +#include #include "rmw/rmw.h" #include "TypeSupport.hpp" -#include "zenoh.h" - /// Structs for various type erased data fields. ///============================================================================== @@ -93,4 +97,4 @@ struct rmw_subscription_data_t bool reliable; }; -#endif // SRC__DETAIL__RMW_DATA_TYPES_HPP +#endif // DETAIL__RMW_DATA_TYPES_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp index 8d0edc8c..f6ec5881 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SRC__DETAIL__RMW_INIT_OPTIONS_IMPL_HPP -#define SRC__DETAIL__RMW_INIT_OPTIONS_IMPL_HPP +#ifndef DETAIL__RMW_INIT_OPTIONS_IMPL_HPP_ +#define DETAIL__RMW_INIT_OPTIONS_IMPL_HPP_ -#include "zenoh.h" +#include // TODO(YV): Consider using this again. struct rmw_init_options_impl_s @@ -24,4 +24,4 @@ struct rmw_init_options_impl_s z_owned_config_t config; }; -#endif // SRC__DETAIL__RMW_INIT_OPTIONS_IMPL_HPP +#endif // DETAIL__RMW_INIT_OPTIONS_IMPL_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/serialization_format.hpp b/rmw_zenoh_cpp/src/detail/serialization_format.hpp index 2c4a3b96..f6f90269 100644 --- a/rmw_zenoh_cpp/src/detail/serialization_format.hpp +++ b/rmw_zenoh_cpp/src/detail/serialization_format.hpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SRC__DETAIL__SERIALIZATION_FORMAT_HPP -#define SRC__DETAIL__SERIALIZATION_FORMAT_HPP +#ifndef DETAIL__SERIALIZATION_FORMAT_HPP_ +#define DETAIL__SERIALIZATION_FORMAT_HPP_ extern inline const char * const rmw_zenoh_serialization_format = "cdr"; -#endif // SRC__DETAIL__SERIALIZATION_FORMAT_HPP +#endif // DETAIL__SERIALIZATION_FORMAT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.hpp b/rmw_zenoh_cpp/src/detail/type_support_common.hpp index 4158e96b..431dc27b 100644 --- a/rmw_zenoh_cpp/src/detail/type_support_common.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support_common.hpp @@ -16,8 +16,8 @@ // This file is originally from: // https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp -#ifndef SRC__DETAIL__TYPE_SUPPORT_COMMON_HPP -#define SRC__DETAIL__TYPE_SUPPORT_COMMON_HPP +#ifndef DETAIL__TYPE_SUPPORT_COMMON_HPP_ +#define DETAIL__TYPE_SUPPORT_COMMON_HPP_ #include #include @@ -66,4 +66,4 @@ _create_type_name( return _create_type_name(message_namespace, message_name); } -#endif // SRC__DETAIL__TYPE_SUPPORT_COMMON_HPP +#endif // DETAIL__TYPE_SUPPORT_COMMON_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index 4dcd418d..aec7927e 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -66,5 +66,4 @@ rmw_event_fini(rmw_event_t * event) (void) event; return RMW_RET_UNSUPPORTED; } - -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp b/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp index 5e0cd285..381f3dbb 100644 --- a/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp @@ -43,5 +43,4 @@ rmw_subscription_get_network_flow_endpoints( (void) network_flow_endpoint_array; return RMW_RET_UNSUPPORTED; } - -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp index 55d417c5..e750e868 100644 --- a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp @@ -70,5 +70,4 @@ rmw_get_client_names_and_types_by_node( { return RMW_RET_UNSUPPORTED; } - -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp index e16f86b2..cadfb282 100644 --- a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp @@ -27,5 +27,4 @@ rmw_get_service_names_and_types( { return RMW_RET_UNSUPPORTED; } - -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp index 298602b0..c7807d3b 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -42,5 +42,4 @@ rmw_get_subscriptions_info_by_topic( { return RMW_RET_OK; } - -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp index d5408e23..c0be4ace 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp @@ -28,5 +28,4 @@ rmw_get_topic_names_and_types( { return RMW_RET_UNSUPPORTED; } - -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 90be17a9..092ab03f 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" #include "detail/rmw_data_types.hpp" - #include "rcutils/env.h" #include "rcutils/strdup.h" #include "rcutils/types.h" @@ -27,8 +28,6 @@ #include "rcpputils/scope_exit.hpp" -#include "zenoh.h" - extern "C" { //============================================================================== @@ -188,4 +187,4 @@ rmw_context_fini(rmw_context_t * context) *context = rmw_get_zero_initialized_context(); return ret; } -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_init_options.cpp b/rmw_zenoh_cpp/src/rmw_init_options.cpp index ffbadfbd..c34a6262 100644 --- a/rmw_zenoh_cpp/src/rmw_init_options.cpp +++ b/rmw_zenoh_cpp/src/rmw_init_options.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "detail/identifier.hpp" #include "detail/rmw_init_options_impl.hpp" @@ -22,8 +24,6 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/init_options.h" -#include "zenoh.h" - extern "C" { //============================================================================== @@ -122,5 +122,4 @@ rmw_init_options_fini(rmw_init_options_t * init_options) return ret; } - -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_qos.cpp b/rmw_zenoh_cpp/src/rmw_qos.cpp index 437aa4e5..539adf54 100644 --- a/rmw_zenoh_cpp/src/rmw_qos.cpp +++ b/rmw_zenoh_cpp/src/rmw_qos.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include "rmw_dds_common/qos.hpp" #include "rmw/types.h" #include "rmw/qos_profiles.h" @@ -30,5 +29,4 @@ rmw_qos_profile_check_compatible( return rmw_dds_common::qos_profile_check_compatible( publisher_profile, subscription_profile, compatibility, reason, reason_size); } - } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 9a6796f9..59373055 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" #include "detail/rmw_data_types.hpp" #include "detail/serialization_format.hpp" #include "detail/type_support_common.hpp" -#include -#include - #include "rcpputils/scope_exit.hpp" #include "rcutils/env.h" @@ -1007,7 +1007,6 @@ rmw_take_with_info( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(subscription->topic_name, RMW_RET_ERROR); RMW_CHECK_ARGUMENT_FOR_NULL(subscription->data, RMW_RET_ERROR); @@ -1299,7 +1298,7 @@ rmw_service_request_subscription_get_actual_qos( const rmw_service_t * service, rmw_qos_profile_t * qos) { - //TODO(yadunund): Fix. + // TODO(yadunund): Fix. return RMW_RET_OK; } @@ -1310,7 +1309,7 @@ rmw_service_response_publisher_get_actual_qos( const rmw_service_t * service, rmw_qos_profile_t * qos) { - //TODO(yadunund): Fix. + // TODO(yadunund): Fix. return RMW_RET_OK; } @@ -1534,7 +1533,8 @@ bool check_wait_conditions( return stop_wait; } -} // namespace anonymous +} // namespace + //============================================================================== /// Waits on sets of different entities and returns when one is ready. rmw_ret_t @@ -1794,5 +1794,4 @@ rmw_event_set_callback( { return RMW_RET_UNSUPPORTED; } - -} // extern "C" +} // extern "C" From 377c6dc8b489f8474b3425421a0cb01ca3126e01 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 21 Sep 2023 08:55:16 -0400 Subject: [PATCH 29/50] Make sure to cleanup in rmw_init as appropriate. (#13) Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/src/rmw_init.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 092ab03f..cf16c113 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -108,6 +108,10 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) RMW_SET_ERROR_MSG("Error setting up zenoh session"); return RMW_RET_INVALID_ARGUMENT; } + auto close_session = rcpputils::make_scope_exit( + [context](){ + z_close(z_move(context->impl->session)); + }); // Initialize the guard condition. context->impl->graph_guard_condition = rmw_guard_condition_allocate(); @@ -125,11 +129,18 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) RMW_SET_ERROR_MSG("Error allocating memory for guard condition data"); return RMW_RET_BAD_ALLOC; } + auto free_guard_condition_data = rcpputils::make_scope_exit( + [context]() { + delete static_cast(context->impl->graph_guard_condition->data); + }); + free_guard_condition_data.cancel(); cleanup_guard_condition.cancel(); + close_session.cancel(); free_options.cancel(); cleanup_impl.cancel(); restore_context.cancel(); + return RMW_RET_OK; } @@ -182,6 +193,10 @@ rmw_context_fini(rmw_context_t * context) rmw_guard_condition_free(context->impl->graph_guard_condition); + if (!context->impl->is_shutdown) { + z_close(z_move(context->impl->session)); + } + rmw_ret_t ret = rmw_init_options_fini(&context->options); delete context->impl; *context = rmw_get_zero_initialized_context(); From bcda4154e8f80cd61339f249c995211f3db55cf0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 21 Sep 2023 08:58:48 -0400 Subject: [PATCH 30/50] Add function to convert ROS topic names to Zenoh keys. (#14) Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/src/rmw_init.cpp | 2 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 33 ++++++++++++++++++++++++++++++++- 2 files changed, 33 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index cf16c113..2f2a4547 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -109,7 +109,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } auto close_session = rcpputils::make_scope_exit( - [context](){ + [context]() { z_close(z_move(context->impl->session)); }); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 59373055..74b5ecd2 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -218,6 +218,34 @@ rmw_fini_publisher_allocation( return RMW_RET_UNSUPPORTED; } +// A function to take ros topic names and convert them to valid Zenoh keys. +// In particular, Zenoh keys cannot start or end with a /, so this function +// will strip them out. +// Performance note: at present, this function allocates a new string and copies +// the old string into it. If this becomes a performance problem, we could consider +// modifying the topic_name in place. But this means we need to be much more +// careful about who owns the string. +static char * ros_topic_name_to_zenoh_key( + const char * const topic_name, rcutils_allocator_t * allocator) +{ + size_t start_offset = 0; + size_t topic_name_len = strlen(topic_name); + size_t end_offset = topic_name_len; + + if (topic_name_len > 0) { + if (topic_name[0] == '/') { + // Strip the leading '/' + start_offset = 1; + } + if (topic_name[end_offset - 1] == '/') { + // Strip the trailing '/' + end_offset -= 1; + } + } + + return rcutils_strndup(&topic_name[start_offset], end_offset - start_offset, *allocator); +} + //============================================================================== /// Create a publisher and return a handle to that publisher. rmw_publisher_t * @@ -333,14 +361,17 @@ rmw_create_publisher( allocator->deallocate(publisher_data, allocator->state); }); - // TODO(yadunund): Zenoh key cannot contain leading or trailing '/' so we strip them. // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate // a z_publisher_put_options struct instead of passing NULL to this function. + char * zenoh_key_name = ros_topic_name_to_zenoh_key(topic_name, allocator); + // TODO(clalancette): Check for NULL + // TODO(clalancette): What happens if the key name is a valid but empty string? publisher_data->pub = z_declare_publisher( z_loan(context_impl->session), z_keyexpr(&topic_name[1]), NULL ); + allocator->deallocate(zenoh_key_name, allocator->state); if (!z_check(publisher_data->pub)) { RMW_SET_ERROR_MSG("unable to create zenoh publisher"); return nullptr; From aeabfc94b9e9a4809c1ed0d1faa3156d6736a29c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 21 Sep 2023 09:39:19 -0400 Subject: [PATCH 31/50] Use the context allocator everywhere. (#15) If the user set it in the context, we should probably honor it. Change most uses in this code to use the context allocator as appropriate. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/src/rmw_init.cpp | 68 ++++++--- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 255 ++++++++++++++++++++------------ 2 files changed, 210 insertions(+), 113 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 2f2a4547..a68c2d4d 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -14,6 +14,8 @@ #include +#include + #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" #include "detail/rmw_data_types.hpp" @@ -64,16 +66,23 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) context->actual_domain_id = RMW_DEFAULT_DOMAIN_ID != options->domain_id ? options->domain_id : 0u; + const rcutils_allocator_t * allocator = &options->allocator; + + // TODO(clalancette): We should use the allocator and placement new here, + // but that caused crashes for reasons I don't understand. context->impl = new (std::nothrow) rmw_context_impl_t(); - if (nullptr == context->impl) { - RMW_SET_ERROR_MSG("failed to allocate context impl"); - return RMW_RET_BAD_ALLOC; - } - auto cleanup_impl = rcpputils::make_scope_exit( - [context]() {delete context->impl;}); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "failed to allocate context impl", + return RMW_RET_BAD_ALLOC); + auto free_impl = rcpputils::make_scope_exit( + [context]() { + delete context->impl; + }); rmw_ret_t ret; if ((ret = rmw_init_options_copy(options, &context->options)) != RMW_RET_OK) { + // error already set return ret; } auto free_options = rcpputils::make_scope_exit( @@ -114,31 +123,44 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) }); // Initialize the guard condition. - context->impl->graph_guard_condition = rmw_guard_condition_allocate(); - if (!context->impl->graph_guard_condition) { - RMW_SET_ERROR_MSG("Error allocating memory for guard condition"); - return RMW_RET_BAD_ALLOC; - } + context->impl->graph_guard_condition = + static_cast(allocator->zero_allocate( + 1, sizeof(rmw_guard_condition_t), allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl->graph_guard_condition, + "failed to allocate graph guard condition", + return RMW_RET_BAD_ALLOC); auto cleanup_guard_condition = rcpputils::make_scope_exit( [context]() { rmw_guard_condition_free(context->impl->graph_guard_condition); }); + context->impl->graph_guard_condition->implementation_identifier = rmw_zenoh_identifier; - context->impl->graph_guard_condition->data = new (std::nothrow) GuardCondition(); - if (!context->impl->graph_guard_condition->data) { - RMW_SET_ERROR_MSG("Error allocating memory for guard condition data"); - return RMW_RET_BAD_ALLOC; - } + + context->impl->graph_guard_condition->data = + allocator->zero_allocate(1, sizeof(GuardCondition), allocator->state); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl->graph_guard_condition->data, + "failed to allocate graph guard condition data", + return RMW_RET_BAD_ALLOC); auto free_guard_condition_data = rcpputils::make_scope_exit( + [context, allocator]() { + allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); + }); + + new(context->impl->graph_guard_condition->data) GuardCondition; + auto destruct_guard_condition = rcpputils::make_scope_exit( [context]() { - delete static_cast(context->impl->graph_guard_condition->data); + auto gc_data = static_cast(context->impl->graph_guard_condition->data); + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(gc_data->~GuardCondition(), GuardCondition); }); + destruct_guard_condition.cancel(); free_guard_condition_data.cancel(); cleanup_guard_condition.cancel(); close_session.cancel(); free_options.cancel(); - cleanup_impl.cancel(); + free_impl.cancel(); restore_context.cancel(); return RMW_RET_OK; @@ -189,17 +211,23 @@ rmw_context_fini(rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } - delete static_cast(context->impl->graph_guard_condition->data); + const rcutils_allocator_t * allocator = &context->options.allocator; - rmw_guard_condition_free(context->impl->graph_guard_condition); + static_cast(context->impl->graph_guard_condition->data)->~GuardCondition(); + allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); + + allocator->deallocate(context->impl->graph_guard_condition, allocator->state); if (!context->impl->is_shutdown) { z_close(z_move(context->impl->session)); } rmw_ret_t ret = rmw_init_options_fini(&context->options); + delete context->impl; + *context = rmw_get_zero_initialized_context(); + return ret; } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 74b5ecd2..d0767ab0 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -15,6 +15,8 @@ #include #include +#include + #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" #include "detail/rmw_data_types.hpp" @@ -115,41 +117,60 @@ rmw_create_node( return nullptr; } - rmw_node_t * node = rmw_node_allocate(); + rcutils_allocator_t * allocator = &context->options.allocator; + + rmw_node_t * node = + static_cast(allocator->zero_allocate(1, sizeof(rmw_node_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( node, "unable to allocate memory for rmw_node_t", return nullptr); auto cleanup_node = rcpputils::make_scope_exit( - [node]() { - rmw_free(const_cast(node->name)); - rmw_free(const_cast(node->namespace_)); - rmw_free(static_cast(node->data)); - rmw_node_free(node); + [node, allocator]() { + allocator->deallocate(node, allocator->state); }); - node->name = static_cast(rmw_allocate(sizeof(char) * strlen(name) + 1)); - RMW_CHECK_ARGUMENT_FOR_NULL(node->name, nullptr); - memcpy(const_cast(node->name), name, strlen(name) + 1); + node->name = rcutils_strdup(name, *allocator); + RMW_CHECK_FOR_NULL_WITH_MSG( + node->name, + "unable to allocate memory for node name", + return nullptr); + auto free_name = rcpputils::make_scope_exit( + [node, allocator]() { + allocator->deallocate(const_cast(node->name), allocator->state); + }); - node->namespace_ = - static_cast(rmw_allocate(sizeof(char) * strlen(namespace_) + 1)); - RMW_CHECK_ARGUMENT_FOR_NULL(node->namespace_, nullptr); - memcpy(const_cast(node->namespace_), namespace_, strlen(namespace_) + 1); + node->namespace_ = rcutils_strdup(namespace_, *allocator); + RMW_CHECK_FOR_NULL_WITH_MSG( + node->namespace_, + "unable to allocate memory for node namespace", + return nullptr); + auto free_namespace = rcpputils::make_scope_exit( + [node, allocator]() { + allocator->deallocate(const_cast(node->namespace_), allocator->state); + }); // TODO(yadunund): Register with storage system here and throw error if // zenohd is not running. // Put metadata into node->data. - auto node_data = static_cast(rmw_allocate(sizeof(rmw_node_data_t))); - if (node_data == nullptr) { - RMW_SET_ERROR_MSG("failed to allocate node data"); - return nullptr; - } - node->data = node_data; + node->data = allocator->zero_allocate(1, sizeof(rmw_node_data_t), allocator->state); + RMW_CHECK_FOR_NULL_WITH_MSG( + node->data, + "unable to allocate memory for node data", + return nullptr); + auto free_node_data = rcpputils::make_scope_exit( + [node, allocator]() { + allocator->deallocate(node->data, allocator->state); + }); - cleanup_node.cancel(); node->implementation_identifier = rmw_zenoh_identifier; node->context = context; + + free_node_data.cancel(); + free_namespace.cancel(); + free_name.cancel(); + cleanup_node.cancel(); + return node; } @@ -158,7 +179,6 @@ rmw_create_node( rmw_ret_t rmw_destroy_node(rmw_node_t * node) { - rmw_ret_t result_ret = RMW_RET_OK; RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, @@ -168,13 +188,14 @@ rmw_destroy_node(rmw_node_t * node) // TODO(Yadunund): Unregister with storage system. - // rmw_context_t * context = node->context; - rmw_free(const_cast(node->name)); - rmw_free(const_cast(node->namespace_)); - rmw_free(static_cast(node->data)); - rmw_node_free(const_cast(node)); - // delete node_impl; - return result_ret; + rcutils_allocator_t * allocator = &node->context->options.allocator; + + allocator->deallocate(const_cast(node->name), allocator->state); + allocator->deallocate(const_cast(node->namespace_), allocator->state); + allocator->deallocate(node->data, allocator->state); + allocator->deallocate(node, allocator->state); + + return RMW_RET_OK; } //============================================================================== @@ -336,9 +357,16 @@ rmw_create_publisher( return nullptr; } + rcutils_allocator_t * allocator = &node->context->options.allocator; + // Create the publisher. - rmw_publisher_t * rmw_publisher = rmw_publisher_allocate(); - RMW_CHECK_ARGUMENT_FOR_NULL(rmw_publisher, nullptr); + auto rmw_publisher = + static_cast(allocator->zero_allocate( + 1, sizeof(rmw_publisher_t), allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + rmw_publisher, + "failed to allocate memory for the publisher", + return nullptr); // Get typed pointer to implementation specific publisher data struct // auto publisher_data = static_cast(rmw_publisher->data); // if (publisher_data == nullptr) { @@ -346,16 +374,16 @@ rmw_create_publisher( // return nullptr; // } auto free_rmw_publisher = rcpputils::make_scope_exit( - [rmw_publisher]() { - rmw_publisher_free(rmw_publisher); + [rmw_publisher, allocator]() { + allocator->deallocate(rmw_publisher, allocator->state); }); - rcutils_allocator_t * allocator = &node->context->options.allocator; + auto publisher_data = static_cast( allocator->allocate(sizeof(rmw_publisher_data_t), allocator->state)); - if (publisher_data == nullptr) { - RMW_SET_ERROR_MSG("failed to allocate publisher data"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher_data, + "failed to allocate memory for publisher data", + return nullptr); auto free_publisher_data = rcpputils::make_scope_exit( [publisher_data, allocator]() { allocator->deallocate(publisher_data, allocator->state); @@ -364,7 +392,10 @@ rmw_create_publisher( // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate // a z_publisher_put_options struct instead of passing NULL to this function. char * zenoh_key_name = ros_topic_name_to_zenoh_key(topic_name, allocator); - // TODO(clalancette): Check for NULL + RMW_CHECK_FOR_NULL_WITH_MSG( + zenoh_key_name, + "failed to allocate memory for zenoh key name", + return nullptr); // TODO(clalancette): What happens if the key name is a valid but empty string? publisher_data->pub = z_declare_publisher( z_loan(context_impl->session), @@ -396,6 +427,13 @@ rmw_create_publisher( }); new(publisher_data->type_support) MessageTypeSupport(callbacks); + auto destruct_msg_type_support = rcpputils::make_scope_exit( + [publisher_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + static_cast(publisher_data->type_support)->~MessageTypeSupport(), + MessageTypeSupport); + }); + publisher_data->context = node->context; rmw_publisher->data = publisher_data; rmw_publisher->implementation_identifier = rmw_zenoh_identifier; @@ -405,10 +443,12 @@ rmw_create_publisher( rmw_publisher->topic_name = rcutils_strdup(topic_name, *allocator); RMW_CHECK_ARGUMENT_FOR_NULL(rmw_publisher->topic_name, nullptr); + destruct_msg_type_support.cancel(); free_type_support.cancel(); undeclare_z_publisher.cancel(); free_publisher_data.cancel(); free_rmw_publisher.cancel(); + return rmw_publisher; } @@ -432,11 +472,12 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) rmw_ret_t ret = RMW_RET_OK; - rmw_free(const_cast(publisher->topic_name)); + rcutils_allocator_t * allocator = &node->context->options.allocator; + + allocator->deallocate(const_cast(publisher->topic_name), allocator->state); auto publisher_data = static_cast(publisher->data); if (publisher_data != nullptr) { - rcutils_allocator_t * allocator = &node->context->options.allocator; allocator->deallocate(publisher_data->type_support, allocator->state); if (z_undeclare_publisher(z_move(publisher_data->pub))) { RMW_SET_ERROR_MSG("failed to undeclare pub"); @@ -444,7 +485,8 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) } allocator->deallocate(publisher_data, allocator->state); } - rmw_publisher_free(publisher); + allocator->deallocate(publisher, allocator->state); + return ret; } @@ -531,11 +573,7 @@ rmw_publish( publisher_data, "publisher_data is null", return RMW_RET_INVALID_ARGUMENT); - // Assign allocator. - // TODO(yadunund): Instead of storing the context in the publisher data to - // retrieve the allocator, use rmw_allocate(). - rcutils_allocator_t * allocator = - &(static_cast(publisher->data)->context->options.allocator); + rcutils_allocator_t * allocator = &(publisher_data->context->options.allocator); // Serialize data. size_t max_data_length = publisher_data->type_support->getEstimatedSerializedSize( @@ -544,6 +582,12 @@ rmw_publish( // Init serialized message byte array char * msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); + auto free_msg_bytes = rcpputils::make_scope_exit( + [msg_bytes, allocator]() { + allocator->deallocate(msg_bytes, allocator->state); + }); // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(msg_bytes, max_data_length); @@ -559,7 +603,6 @@ rmw_publish( publisher_data->type_support_impl)) { RMW_SET_ERROR_MSG("could not serialize ROS message"); - allocator->deallocate(msg_bytes, allocator->state); return RMW_RET_ERROR; } @@ -577,8 +620,6 @@ rmw_publish( data_length, &options); - allocator->deallocate(msg_bytes, allocator->state); - if (ret) { RMW_SET_ERROR_MSG("unable to publish message"); return RMW_RET_ERROR; @@ -1349,10 +1390,45 @@ rmw_service_response_publisher_get_actual_qos( rmw_guard_condition_t * rmw_create_guard_condition(rmw_context_t * context) { - rmw_guard_condition_t * guard_condition_handle = rmw_guard_condition_allocate(); - guard_condition_handle->implementation_identifier = rmw_zenoh_identifier; - guard_condition_handle->data = new GuardCondition(); - return guard_condition_handle; + rcutils_allocator_t * allocator = &context->options.allocator; + + auto guard_condition = + static_cast(allocator->zero_allocate( + 1, sizeof(rmw_guard_condition_t), allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + guard_condition, + "unable to allocate memory for guard_condition", + return nullptr); + auto free_guard_condition = rcpputils::make_scope_exit( + [guard_condition, allocator]() { + allocator->deallocate(guard_condition, allocator->state); + }); + + guard_condition->implementation_identifier = rmw_zenoh_identifier; + guard_condition->context = context; + + guard_condition->data = allocator->zero_allocate(1, sizeof(GuardCondition), allocator->state); + RMW_CHECK_FOR_NULL_WITH_MSG( + guard_condition->data, + "unable to allocate memory for guard condition data", + return nullptr); + auto free_guard_condition_data = rcpputils::make_scope_exit( + [guard_condition, allocator]() { + allocator->deallocate(guard_condition->data, allocator->state); + }); + + new(guard_condition->data) GuardCondition; + auto destruct_guard_condition = rcpputils::make_scope_exit( + [guard_condition]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + static_cast(guard_condition->data)->~GuardCondition(), GuardCondition); + }); + + destruct_guard_condition.cancel(); + free_guard_condition_data.cancel(); + free_guard_condition.cancel(); + + return guard_condition; } /// Finalize a given guard condition handle, reclaim the resources, and deallocate the handle. @@ -1361,10 +1437,15 @@ rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) { RMW_CHECK_ARGUMENT_FOR_NULL(guard_condition, RMW_RET_INVALID_ARGUMENT); + rcutils_allocator_t * allocator = &guard_condition->context->options.allocator; + if (guard_condition->data) { - delete static_cast(guard_condition->data); + static_cast(guard_condition->data)->~GuardCondition(); + allocator->deallocate(guard_condition->data, allocator->state); } - rmw_guard_condition_free(guard_condition); + + allocator->deallocate(guard_condition, allocator->state); + return RMW_RET_OK; } @@ -1380,6 +1461,7 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition) return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); static_cast(guard_condition->data)->trigger(); + return RMW_RET_OK; } @@ -1397,35 +1479,38 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) return nullptr); rmw_wait_set_t * wait_set = rmw_wait_set_allocate(); - - auto cleanup_wait_set = rcpputils::make_scope_exit( - [wait_set]() { - if (wait_set) { - if (wait_set->data) { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - static_cast(wait_set->data)->~rmw_wait_set_data_t(), - wait_set->data); - rmw_free(wait_set->data); - } - rmw_wait_set_free(wait_set); - } - }); - - // From here onward, error results in unrolling in the goto fail block. if (!wait_set) { RMW_SET_ERROR_MSG("failed to allocate wait set"); return nullptr; } + auto cleanup_wait_set = rcpputils::make_scope_exit( + [wait_set]() { + rmw_wait_set_free(wait_set); + }); + wait_set->implementation_identifier = rmw_zenoh_identifier; - wait_set->data = rmw_allocate(sizeof(rmw_wait_set_data_t)); - // Invoke placement new - new(wait_set->data) rmw_wait_set_data_t; + wait_set->data = rmw_allocate(sizeof(rmw_wait_set_data_t)); if (!wait_set->data) { RMW_SET_ERROR_MSG("failed to construct wait set info struct"); return nullptr; } + auto free_wait_set_data = rcpputils::make_scope_exit( + [wait_set]() { + rmw_free(wait_set->data); + }); + // Invoke placement new + new(wait_set->data) rmw_wait_set_data_t; + auto destruct_rmw_wait_set_data = rcpputils::make_scope_exit( + [wait_set]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + static_cast(wait_set->data)->~rmw_wait_set_data_t(), + rmw_wait_set_data); + }); + + destruct_rmw_wait_set_data.cancel(); + free_wait_set_data.cancel(); cleanup_wait_set.cancel(); return wait_set; } @@ -1443,30 +1528,14 @@ rmw_destroy_wait_set(rmw_wait_set_t * wait_set) rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - rmw_ret_t result = RMW_RET_OK; auto wait_set_data = static_cast(wait_set->data); - if (!wait_set_data) { - RMW_SET_ERROR_MSG("wait set info is null"); - return RMW_RET_ERROR; - } - std::mutex * conditionMutex = &wait_set_data->condition_mutex; - if (!conditionMutex) { - RMW_SET_ERROR_MSG("wait set mutex is null"); - return RMW_RET_ERROR; - } - - if (wait_set->data) { - if (wait_set_data) { - RMW_TRY_DESTRUCTOR( - wait_set_data->~rmw_wait_set_data_t(), - wait_set_data, result = RMW_RET_ERROR); - } - rmw_free(wait_set->data); - } + wait_set_data->~rmw_wait_set_data_t(); + rmw_free(wait_set->data); rmw_wait_set_free(wait_set); - return result; + + return RMW_RET_OK; } //============================================================================== From 77e4e9ac4e109d0e2fc49d2a9782d1b555112077 Mon Sep 17 00:00:00 2001 From: Yadu Date: Thu, 21 Sep 2023 22:30:28 +0800 Subject: [PATCH 32/50] use zenoh_key_name (#16) Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index d0767ab0..04b36e91 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -399,7 +399,7 @@ rmw_create_publisher( // TODO(clalancette): What happens if the key name is a valid but empty string? publisher_data->pub = z_declare_publisher( z_loan(context_impl->session), - z_keyexpr(&topic_name[1]), + z_keyexpr(zenoh_key_name), NULL ); allocator->deallocate(zenoh_key_name, allocator->state); From 9183af2ff336f59483376a8149de4f257338713c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 21 Sep 2023 12:08:50 -0400 Subject: [PATCH 33/50] Remove no-unused-parameter skip. (#17) And fix up everything so that we properly skip them in the code. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 3 - .../src/rmw_get_node_info_and_types.cpp | 22 +++ .../src/rmw_get_service_names_and_types.cpp | 3 + .../src/rmw_get_topic_endpoint_info.cpp | 10 ++ .../src/rmw_get_topic_names_and_types.cpp | 4 + rmw_zenoh_cpp/src/rmw_zenoh.cpp | 144 +++++++++++++++++- 6 files changed, 177 insertions(+), 9 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index a9e55ccf..6160b316 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -11,9 +11,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# TODO(yadunund): Remove this eventually. -add_compile_options(-Wno-unused-parameter) - # find dependencies find_package(ament_cmake REQUIRED) diff --git a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp index e750e868..cd16fd6d 100644 --- a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp @@ -28,6 +28,12 @@ rmw_get_subscriber_names_and_types_by_node( bool no_demangle, rmw_names_and_types_t * topic_names_and_types) { + static_cast(node); + static_cast(allocator); + static_cast(node_name); + static_cast(node_namespace); + static_cast(no_demangle); + static_cast(topic_names_and_types); return RMW_RET_UNSUPPORTED; } @@ -42,6 +48,12 @@ rmw_get_publisher_names_and_types_by_node( bool no_demangle, rmw_names_and_types_t * topic_names_and_types) { + static_cast(node); + static_cast(allocator); + static_cast(node_name); + static_cast(node_namespace); + static_cast(no_demangle); + static_cast(topic_names_and_types); return RMW_RET_UNSUPPORTED; } @@ -55,6 +67,11 @@ rmw_get_service_names_and_types_by_node( const char * node_namespace, rmw_names_and_types_t * service_names_and_types) { + static_cast(node); + static_cast(allocator); + static_cast(node_name); + static_cast(node_namespace); + static_cast(service_names_and_types); return RMW_RET_UNSUPPORTED; } @@ -68,6 +85,11 @@ rmw_get_client_names_and_types_by_node( const char * node_namespace, rmw_names_and_types_t * service_names_and_types) { + static_cast(node); + static_cast(allocator); + static_cast(node_name); + static_cast(node_namespace); + static_cast(service_names_and_types); return RMW_RET_UNSUPPORTED; } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp index cadfb282..ae8e54c4 100644 --- a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp @@ -25,6 +25,9 @@ rmw_get_service_names_and_types( rcutils_allocator_t * allocator, rmw_names_and_types_t * service_names_and_types) { + static_cast(node); + static_cast(allocator); + static_cast(service_names_and_types); return RMW_RET_UNSUPPORTED; } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp index c7807d3b..bf435678 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -27,6 +27,11 @@ rmw_get_publishers_info_by_topic( bool no_mangle, rmw_topic_endpoint_info_array_t * publishers_info) { + static_cast(node); + static_cast(allocator); + static_cast(topic_name); + static_cast(no_mangle); + static_cast(publishers_info); return RMW_RET_OK; } @@ -40,6 +45,11 @@ rmw_get_subscriptions_info_by_topic( bool no_mangle, rmw_topic_endpoint_info_array_t * subscriptions_info) { + static_cast(node); + static_cast(allocator); + static_cast(topic_name); + static_cast(no_mangle); + static_cast(subscriptions_info); return RMW_RET_OK; } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp index c0be4ace..3260c4aa 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp @@ -26,6 +26,10 @@ rmw_get_topic_names_and_types( bool no_demangle, rmw_names_and_types_t * topic_names_and_types) { + static_cast(node); + static_cast(allocator); + static_cast(no_demangle); + static_cast(topic_names_and_types); return RMW_RET_UNSUPPORTED; } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 04b36e91..acc69617 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -498,6 +498,10 @@ rmw_take_dynamic_message( bool * taken, rmw_subscription_allocation_t * allocation) { + static_cast(subscription); + static_cast(dynamic_message); + static_cast(taken); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -510,6 +514,11 @@ rmw_take_dynamic_message_with_info( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation) { + static_cast(subscription); + static_cast(dynamic_message); + static_cast(taken); + static_cast(message_info); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -520,6 +529,9 @@ rmw_serialization_support_init( rcutils_allocator_t * allocator, rosidl_dynamic_typesupport_serialization_support_t * serialization_support) { + static_cast(serialization_lib_name); + static_cast(allocator); + static_cast(serialization_support); return RMW_RET_UNSUPPORTED; } @@ -731,6 +743,9 @@ rmw_get_serialized_message_size( const rosidl_runtime_c__Sequence__bound * message_bounds, size_t * size) { + static_cast(type_support); + static_cast(message_bounds); + static_cast(size); return RMW_RET_UNSUPPORTED; } @@ -739,6 +754,7 @@ rmw_get_serialized_message_size( rmw_ret_t rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) { + static_cast(publisher); return RMW_RET_UNSUPPORTED; } @@ -749,6 +765,8 @@ rmw_publisher_wait_for_all_acked( const rmw_publisher_t * publisher, rmw_time_t wait_timeout) { + static_cast(publisher); + static_cast(wait_timeout); return RMW_RET_UNSUPPORTED; } @@ -760,6 +778,9 @@ rmw_serialize( const rosidl_message_type_support_t * type_support, rmw_serialized_message_t * serialized_message) { + static_cast(ros_message); + static_cast(type_support); + static_cast(serialized_message); return RMW_RET_UNSUPPORTED; } @@ -771,6 +792,9 @@ rmw_deserialize( const rosidl_message_type_support_t * type_support, void * ros_message) { + static_cast(serialized_message); + static_cast(type_support); + static_cast(ros_message); return RMW_RET_UNSUPPORTED; } @@ -783,9 +807,9 @@ rmw_init_subscription_allocation( rmw_subscription_allocation_t * allocation) { // Unused in current implementation. - (void) type_support; - (void) message_bounds; - (void) allocation; + static_cast(type_support); + static_cast(message_bounds); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -795,7 +819,7 @@ rmw_ret_t rmw_fini_subscription_allocation( rmw_subscription_allocation_t * allocation) { - (void) allocation; + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -1010,6 +1034,8 @@ rmw_subscription_count_matched_publishers( const rmw_subscription_t * subscription, size_t * publisher_count) { + static_cast(subscription); + static_cast(publisher_count); return RMW_RET_UNSUPPORTED; } @@ -1043,6 +1069,8 @@ rmw_subscription_set_content_filter( rmw_subscription_t * subscription, const rmw_subscription_content_filter_options_t * options) { + static_cast(subscription); + static_cast(options); return RMW_RET_UNSUPPORTED; } @@ -1054,6 +1082,9 @@ rmw_subscription_get_content_filter( rcutils_allocator_t * allocator, rmw_subscription_content_filter_options_t * options) { + static_cast(subscription); + static_cast(allocator); + static_cast(options); return RMW_RET_UNSUPPORTED; } @@ -1066,6 +1097,10 @@ rmw_take( bool * taken, rmw_subscription_allocation_t * allocation) { + static_cast(subscription); + static_cast(ros_message); + static_cast(taken); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -1090,7 +1125,7 @@ rmw_take_with_info( subscription->implementation_identifier, rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - (void) allocation; + static_cast(allocation); *taken = false; auto * sub_data = static_cast(subscription->data); @@ -1148,6 +1183,12 @@ rmw_take_sequence( size_t * taken, rmw_subscription_allocation_t * allocation) { + static_cast(subscription); + static_cast(count); + static_cast(message_sequence); + static_cast(message_info_sequence); + static_cast(taken); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -1160,6 +1201,10 @@ rmw_take_serialized_message( bool * taken, rmw_subscription_allocation_t * allocation) { + static_cast(subscription); + static_cast(serialized_message); + static_cast(taken); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -1173,6 +1218,11 @@ rmw_take_serialized_message_with_info( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation) { + static_cast(subscription); + static_cast(serialized_message); + static_cast(taken); + static_cast(message_info); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -1185,6 +1235,10 @@ rmw_take_loaned_message( bool * taken, rmw_subscription_allocation_t * allocation) { + static_cast(subscription); + static_cast(loaned_message); + static_cast(taken); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -1198,6 +1252,11 @@ rmw_take_loaned_message_with_info( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation) { + static_cast(subscription); + static_cast(loaned_message); + static_cast(taken); + static_cast(message_info); + static_cast(allocation); return RMW_RET_UNSUPPORTED; } @@ -1208,6 +1267,8 @@ rmw_return_loaned_message_from_subscription( const rmw_subscription_t * subscription, void * loaned_message) { + static_cast(subscription); + static_cast(loaned_message); return RMW_RET_UNSUPPORTED; } @@ -1220,6 +1281,10 @@ rmw_create_client( const char * service_name, const rmw_qos_profile_t * qos_profile) { + static_cast(node); + static_cast(type_support); + static_cast(service_name); + static_cast(qos_profile); return nullptr; } @@ -1228,6 +1293,8 @@ rmw_create_client( rmw_ret_t rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) { + static_cast(node); + static_cast(client); return RMW_RET_UNSUPPORTED; } @@ -1239,6 +1306,9 @@ rmw_send_request( const void * ros_request, int64_t * sequence_id) { + static_cast(client); + static_cast(ros_request); + static_cast(sequence_id); return RMW_RET_UNSUPPORTED; } @@ -1251,6 +1321,10 @@ rmw_take_response( void * ros_response, bool * taken) { + static_cast(client); + static_cast(request_header); + static_cast(ros_response); + static_cast(taken); return RMW_RET_UNSUPPORTED; } @@ -1261,6 +1335,8 @@ rmw_client_request_publisher_get_actual_qos( const rmw_client_t * client, rmw_qos_profile_t * qos) { + static_cast(client); + static_cast(qos); return RMW_RET_UNSUPPORTED; } @@ -1271,6 +1347,8 @@ rmw_client_response_subscription_get_actual_qos( const rmw_client_t * client, rmw_qos_profile_t * qos) { + static_cast(client); + static_cast(qos); return RMW_RET_UNSUPPORTED; } @@ -1349,6 +1427,10 @@ rmw_take_request( void * ros_request, bool * taken) { + static_cast(service); + static_cast(request_header); + static_cast(ros_request); + static_cast(taken); return RMW_RET_UNSUPPORTED; } @@ -1360,6 +1442,9 @@ rmw_send_response( rmw_request_id_t * request_header, void * ros_response) { + static_cast(service); + static_cast(request_header); + static_cast(ros_response); return RMW_RET_UNSUPPORTED; } @@ -1371,6 +1456,8 @@ rmw_service_request_subscription_get_actual_qos( rmw_qos_profile_t * qos) { // TODO(yadunund): Fix. + static_cast(service); + static_cast(qos); return RMW_RET_OK; } @@ -1382,6 +1469,8 @@ rmw_service_response_publisher_get_actual_qos( rmw_qos_profile_t * qos) { // TODO(yadunund): Fix. + static_cast(service); + static_cast(qos); return RMW_RET_OK; } @@ -1470,7 +1559,8 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition) rmw_wait_set_t * rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) { - (void) max_conditions; + static_cast(max_conditions); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( context, @@ -1749,6 +1839,9 @@ rmw_get_node_names( rcutils_string_array_t * node_names, rcutils_string_array_t * node_namespaces) { + static_cast(node); + static_cast(node_names); + static_cast(node_namespaces); return RMW_RET_UNSUPPORTED; } @@ -1761,6 +1854,10 @@ rmw_get_node_names_with_enclaves( rcutils_string_array_t * node_namespaces, rcutils_string_array_t * enclaves) { + static_cast(node); + static_cast(node_names); + static_cast(node_namespaces); + static_cast(enclaves); return RMW_RET_UNSUPPORTED; } @@ -1772,6 +1869,9 @@ rmw_count_publishers( const char * topic_name, size_t * count) { + static_cast(node); + static_cast(topic_name); + static_cast(count); return RMW_RET_UNSUPPORTED; } @@ -1783,6 +1883,9 @@ rmw_count_subscribers( const char * topic_name, size_t * count) { + static_cast(node); + static_cast(topic_name); + static_cast(count); return RMW_RET_UNSUPPORTED; } @@ -1794,6 +1897,9 @@ rmw_count_clients( const char * service_name, size_t * count) { + static_cast(node); + static_cast(service_name); + static_cast(count); return RMW_RET_UNSUPPORTED; } @@ -1805,6 +1911,9 @@ rmw_count_services( const char * service_name, size_t * count) { + static_cast(node); + static_cast(service_name); + static_cast(count); return RMW_RET_UNSUPPORTED; } @@ -1813,6 +1922,8 @@ rmw_count_services( rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) { + static_cast(publisher); + static_cast(gid); return RMW_RET_UNSUPPORTED; } @@ -1821,6 +1932,8 @@ rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) rmw_ret_t rmw_get_gid_for_client(const rmw_client_t * client, rmw_gid_t * gid) { + static_cast(client); + static_cast(gid); return RMW_RET_UNSUPPORTED; } @@ -1829,6 +1942,9 @@ rmw_get_gid_for_client(const rmw_client_t * client, rmw_gid_t * gid) rmw_ret_t rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * result) { + static_cast(gid1); + static_cast(gid2); + static_cast(result); return RMW_RET_UNSUPPORTED; } @@ -1840,6 +1956,9 @@ rmw_service_server_is_available( const rmw_client_t * client, bool * is_available) { + static_cast(node); + static_cast(client); + static_cast(is_available); return RMW_RET_UNSUPPORTED; } @@ -1848,6 +1967,7 @@ rmw_service_server_is_available( rmw_ret_t rmw_set_log_severity(rmw_log_severity_t severity) { + static_cast(severity); return RMW_RET_UNSUPPORTED; } @@ -1859,6 +1979,9 @@ rmw_subscription_set_on_new_message_callback( rmw_event_callback_t callback, const void * user_data) { + static_cast(subscription); + static_cast(callback); + static_cast(user_data); return RMW_RET_UNSUPPORTED; } @@ -1870,6 +1993,9 @@ rmw_service_set_on_new_request_callback( rmw_event_callback_t callback, const void * user_data) { + static_cast(service); + static_cast(callback); + static_cast(user_data); return RMW_RET_UNSUPPORTED; } @@ -1881,6 +2007,9 @@ rmw_client_set_on_new_response_callback( rmw_event_callback_t callback, const void * user_data) { + static_cast(client); + static_cast(callback); + static_cast(user_data); return RMW_RET_UNSUPPORTED; } @@ -1892,6 +2021,9 @@ rmw_event_set_callback( rmw_event_callback_t callback, const void * user_data) { + static_cast(event); + static_cast(callback); + static_cast(user_data); return RMW_RET_UNSUPPORTED; } } // extern "C" From f57c4aaa3391aab24aae032dfb476096fae6a47b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Sep 2023 15:47:10 -0400 Subject: [PATCH 34/50] Make subscriptions work! (#18) * rmw_wait * Implement more of events. * Cleanups to subscriptions. * Get subscriptions working! * Cleanup GuardCondition API * Make sure not to throw exceptions through the rmw. * Small revamp to reuse code between rmw_take and rmw_take_with_info Signed-off-by: Chris Lalancette Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/guard_condition.cpp | 32 +- rmw_zenoh_cpp/src/detail/guard_condition.hpp | 11 +- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 73 ++- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 20 +- rmw_zenoh_cpp/src/rmw_event.cpp | 89 ++- .../src/rmw_get_topic_names_and_types.cpp | 65 +- rmw_zenoh_cpp/src/rmw_init.cpp | 55 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 613 +++++++++--------- 8 files changed, 554 insertions(+), 404 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.cpp b/rmw_zenoh_cpp/src/detail/guard_condition.cpp index 7ce3f0b3..91509570 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.cpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.cpp @@ -18,34 +18,29 @@ ///============================================================================== GuardCondition::GuardCondition() : has_triggered_(false), - condition_mutex_(nullptr), - condition_variable_(nullptr) {} + condition_variable_(nullptr) +{ +} ///============================================================================== void GuardCondition::trigger() { std::lock_guard lock(internal_mutex_); - if (condition_mutex_ != nullptr) { - std::unique_lock clock(*condition_mutex_); - // the change to hasTriggered_ needs to be mutually exclusive with - // rmw_wait() which checks hasTriggered() and decides if wait() needs to - // be called - has_triggered_ = true; - clock.unlock(); + // the change to hasTriggered_ needs to be mutually exclusive with + // rmw_wait() which checks hasTriggered() and decides if wait() needs to + // be called + has_triggered_ = true; + + if (condition_variable_ != nullptr) { condition_variable_->notify_one(); - } else { - has_triggered_ = true; } } ///============================================================================== -void GuardCondition::attach_condition( - std::mutex * condition_mutex, - std::condition_variable * condition_variable) +void GuardCondition::attach_condition(std::condition_variable * condition_variable) { std::lock_guard lock(internal_mutex_); - condition_mutex_ = condition_mutex; condition_variable_ = condition_variable; } @@ -53,20 +48,19 @@ void GuardCondition::attach_condition( void GuardCondition::detach_condition() { std::lock_guard lock(internal_mutex_); - condition_mutex_ = nullptr; condition_variable_ = nullptr; } ///============================================================================== bool GuardCondition::has_triggered() const { + std::lock_guard lock(internal_mutex_); return has_triggered_; } ///============================================================================== -bool GuardCondition::get_has_triggered() +void GuardCondition::reset_trigger() { - bool ret = has_triggered_; + std::lock_guard lock(internal_mutex_); has_triggered_ = false; - return ret; } diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.hpp b/rmw_zenoh_cpp/src/detail/guard_condition.hpp index a1c1b79c..bbd81b7f 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.hpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.hpp @@ -26,22 +26,21 @@ class GuardCondition final public: GuardCondition(); + // Sets has_triggered_ to true and calls notify_one() on condition_variable_ if set. void trigger(); - void attach_condition( - std::mutex * condition_mutex, - std::condition_variable * condition_variable); + void attach_condition(std::condition_variable * condition_variable); void detach_condition(); bool has_triggered() const; - bool get_has_triggered(); + // Resets has_triggered_ to false. + void reset_trigger(); private: - std::mutex internal_mutex_; + mutable std::mutex internal_mutex_; std::atomic_bool has_triggered_; - std::mutex * condition_mutex_; std::condition_variable * condition_variable_; }; diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index eb9901c2..40e96e10 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + +#include #include -#include -#include "rmw_data_types.hpp" +#include "rcutils/logging_macros.h" -std::mutex sub_callback_mutex; +#include "rmw_data_types.hpp" ///============================================================================== void sub_data_handler( @@ -26,38 +27,58 @@ void sub_data_handler( void * data) { z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); - // TODO(yadunund): Remove after debugging. - printf( - ">> [Subscriber] Received ('%s': size: '%i', payload: '%s')\n", z_loan(keystr), - static_cast(sample->payload.len), sample->payload.start); auto sub_data = static_cast(data); if (sub_data == nullptr) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Unable to obtain rmw_subscription_data_t from data for " + "subscription for %s", + z_loan(keystr) + ); return; } - std::lock_guard guard(sub_callback_mutex); - // Vector to store the byte array (so we have a copyable container instead of a pointer) - std::vector byte_vec(sample->payload.start, - sample->payload.start + sample->payload.len); - auto byte_vec_ptr = std::make_shared>(std::move(byte_vec)); - - std::unique_lock lock(sub_data->message_queue_mutex); + rcutils_allocator_t * allocator = &sub_data->context->options.allocator; - if (sub_data->message_queue.size() >= sub_data->queue_depth) { - // Log warning if message is discarded due to hitting the queue depth - RCUTILS_LOG_WARN_NAMED( + uint8_t * cdr_buffer = + static_cast(allocator->allocate(sample->payload.len, allocator->state)); + if (cdr_buffer == nullptr) { + RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", - "Message queue depth of %ld reached, discarding oldest message " - "for subscription for %s", - sub_data->queue_depth, - z_loan(keystr)); - - sub_data->message_queue.pop_back(); + "Failed to allocate memory for cdr_buffer for " + "subscription for %s", + z_loan(keystr) + ); + return; } - sub_data->message_queue.push_front(byte_vec_ptr); + memcpy(cdr_buffer, sample->payload.start, sample->payload.len); + + { + std::lock_guard lock(sub_data->message_queue_mutex); + + if (sub_data->message_queue.size() >= sub_data->queue_depth) { + // Log warning if message is discarded due to hitting the queue depth + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "Message queue depth of %ld reached, discarding oldest message " + "for subscription for %s", + sub_data->queue_depth, + z_loan(keystr)); - sub_data->message_queue_mutex.unlock(); + std::pair old = sub_data->message_queue.back(); + allocator->deallocate(old.second, allocator->state); + sub_data->message_queue.pop_back(); + } + + sub_data->message_queue.push_front(std::make_pair(sample->payload.len, cdr_buffer)); + + // Since we added new data, trigger the guard condition if it is available + std::lock_guard internal_lock(sub_data->internal_mutex); + if (sub_data->condition != nullptr) { + sub_data->condition->notify_one(); + } + } z_drop(z_move(keystr)); } diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 9cc2cbd4..3e91c421 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -23,11 +23,13 @@ #include #include #include -#include +#include + +#include "rcutils/allocator.h" #include "rmw/rmw.h" -#include "TypeSupport.hpp" +#include "MessageTypeSupport.hpp" /// Structs for various type erased data fields. @@ -64,7 +66,7 @@ struct rmw_publisher_data_t // Type support fields const void * type_support_impl; const char * typesupport_identifier; - TypeSupport * type_support; + MessageTypeSupport * type_support; // Context for memory allocation for messages. rmw_context_t * context; @@ -73,8 +75,10 @@ struct rmw_publisher_data_t ///============================================================================== struct rmw_wait_set_data_t { - std::condition_variable condition; + std::condition_variable condition_variable; std::mutex condition_mutex; + + rmw_context_t * context; }; ///============================================================================== @@ -88,13 +92,17 @@ struct rmw_subscription_data_t const void * type_support_impl; const char * typesupport_identifier; - TypeSupport * type_support; + MessageTypeSupport * type_support; + rmw_context_t * context; - std::deque>> message_queue; + std::deque> message_queue; std::mutex message_queue_mutex; size_t queue_depth; bool reliable; + + std::mutex internal_mutex; + std::condition_variable * condition{nullptr}; }; #endif // DETAIL__RMW_DATA_TYPES_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index aec7927e..d547e4c0 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -12,10 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + +#include "rmw/error_handling.h" #include "rmw/event.h" +#include "detail/identifier.hpp" + extern "C" { +static const std::unordered_map event_map{ + // TODO(clalancette): Implement some events +}; + ///============================================================================== /// Initialize a rmw subscription event rmw_ret_t @@ -24,10 +33,27 @@ rmw_publisher_event_init( const rmw_publisher_t * publisher, rmw_event_type_t event_type) { - (void) rmw_event; - (void) publisher; - (void) event_type; - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(publisher->implementation_identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(publisher->data, RMW_RET_INVALID_ARGUMENT); + + if (publisher->implementation_identifier != rmw_zenoh_identifier) { + RMW_SET_ERROR_MSG("Publisher implementation identifier not from this implementation"); + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION; + } + + if (event_map.count(event_type) != 1) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "provided event_type %d is not supported by rmw_zenoh_cpp", event_type); + return RMW_RET_UNSUPPORTED; + } + + rmw_event->implementation_identifier = publisher->implementation_identifier; + rmw_event->data = publisher->data; + rmw_event->event_type = event_type; + + return RMW_RET_OK; } ///============================================================================== @@ -38,10 +64,28 @@ rmw_subscription_event_init( const rmw_subscription_t * subscription, rmw_event_type_t event_type) { - (void) rmw_event; - (void) subscription; - (void) event_type; - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription->implementation_identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription->data, RMW_RET_INVALID_ARGUMENT); + + if (subscription->implementation_identifier != rmw_zenoh_identifier) { + RMW_SET_ERROR_MSG( + "Subscription implementation identifier not from this implementation"); + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION; + } + + if (event_map.count(event_type) != 1) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "provided event_type %d is not supported by rmw_zenoh_cpp", event_type); + return RMW_RET_UNSUPPORTED; + } + + rmw_event->implementation_identifier = subscription->implementation_identifier; + rmw_event->data = subscription->data; + rmw_event->event_type = event_type; + + return RMW_RET_OK; } ///============================================================================== @@ -51,19 +95,24 @@ rmw_take_event( void * event_info, bool * taken) { - (void) event_handle; - (void) event_info; - (void) taken; - return RMW_RET_UNSUPPORTED; -} + RMW_CHECK_ARGUMENT_FOR_NULL(event_handle, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(event_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + if (event_handle->implementation_identifier != rmw_zenoh_identifier) { + RMW_SET_ERROR_MSG( + "Event implementation identifier not from this implementation"); + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION; + } -///============================================================================== -/// Finalize an rmw_event_t. -rmw_ret_t -rmw_event_fini(rmw_event_t * event) -{ - (void) event; - return RMW_RET_UNSUPPORTED; + switch (event_handle->event_type) { + case RMW_EVENT_INVALID: + break; + default: + break; + } + + *taken = false; + return RMW_RET_ERROR; } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp index 3260c4aa..7e4f2645 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp @@ -12,13 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rcutils/strdup.h" +#include "rmw/error_handling.h" #include "rmw/get_topic_names_and_types.h" +#include "rcpputils/scope_exit.hpp" + extern "C" { ///============================================================================== /// Return all topic names and types in the ROS graph. +// TODO(yadunund): Fix implementation once discovery information can be cached. rmw_ret_t rmw_get_topic_names_and_types( const rmw_node_t * node, @@ -27,9 +32,63 @@ rmw_get_topic_names_and_types( rmw_names_and_types_t * topic_names_and_types) { static_cast(node); - static_cast(allocator); static_cast(no_demangle); - static_cast(topic_names_and_types); - return RMW_RET_UNSUPPORTED; + + rmw_ret_t ret = rmw_names_and_types_init(topic_names_and_types, 1, allocator); + if (ret != RMW_RET_OK) { + return ret; + } + auto cleanup_names_and_types = rcpputils::make_scope_exit( + [topic_names_and_types] { + rmw_ret_t fail_ret = rmw_names_and_types_fini(topic_names_and_types); + if (fail_ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR("failed to cleanup names and types during error handling"); + } + }); + + // topic_names_and_types->names is an rcutils_string_array_t, + // while topic_names_and_types->types is an rcutils_string_array_t * + + topic_names_and_types->names.data[0] = rcutils_strdup("/chatter", *allocator); + if (topic_names_and_types->names.data[0] == nullptr) { + RMW_SET_ERROR_MSG("failed to allocate memory for topic names"); + return RMW_RET_BAD_ALLOC; + } + auto free_names = rcpputils::make_scope_exit( + [topic_names_and_types, allocator] { + allocator->deallocate(topic_names_and_types->names.data[0], allocator->state); + }); + + rcutils_ret_t rcutils_ret = rcutils_string_array_init( + topic_names_and_types->types, 1, + allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + RMW_SET_ERROR_MSG("failed to allocate memory for topic types"); + return RMW_RET_BAD_ALLOC; + } + auto fini_string_array = rcpputils::make_scope_exit( + [topic_names_and_types] { + rmw_ret_t fail_ret = rcutils_string_array_fini(topic_names_and_types->types); + if (fail_ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR("failed to cleanup topic types during error handling"); + } + }); + + topic_names_and_types->types[0].data[0] = rcutils_strdup("std_msgs/msg/String", *allocator); + if (topic_names_and_types->types[0].data[0] == nullptr) { + RMW_SET_ERROR_MSG("failed to allocate memory for topic data"); + return RMW_RET_BAD_ALLOC; + } + auto free_types = rcpputils::make_scope_exit( + [topic_names_and_types, allocator] { + allocator->deallocate(topic_names_and_types->types[0].data[0], allocator->state); + }); + + free_types.cancel(); + fini_string_array.cancel(); + free_names.cancel(); + cleanup_names_and_types.cancel(); + + return RMW_RET_OK; } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index a68c2d4d..c5a891a5 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -37,6 +37,7 @@ extern "C" rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) { + fprintf(stderr, "Initializing context\n"); RMW_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_FOR_NULL_WITH_MSG( @@ -68,16 +69,22 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) const rcutils_allocator_t * allocator = &options->allocator; - // TODO(clalancette): We should use the allocator and placement new here, - // but that caused crashes for reasons I don't understand. - context->impl = new (std::nothrow) rmw_context_impl_t(); + context->impl = static_cast( + allocator->zero_allocate(1, sizeof(rmw_context_impl_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( context->impl, "failed to allocate context impl", return RMW_RET_BAD_ALLOC); auto free_impl = rcpputils::make_scope_exit( - [context]() { - delete context->impl; + [context, allocator]() { + allocator->deallocate(context->impl, allocator->state); + }); + + RMW_TRY_PLACEMENT_NEW(context->impl, context->impl, return RMW_RET_BAD_ALLOC, rmw_context_impl_t); + auto impl_destructor = rcpputils::make_scope_exit( + [context] { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + context->impl->~rmw_context_impl_t(), rmw_context_impl_t); }); rmw_ret_t ret; @@ -87,7 +94,10 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) } auto free_options = rcpputils::make_scope_exit( [context]() { - rmw_ret_t ret = rmw_init_options_fini(&context->options); (void)ret; + rmw_ret_t ret = rmw_init_options_fini(&context->options); + if (ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR("Failed to cleanup context options during error handling"); + } }); // Initialize context's implementation @@ -111,6 +121,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } } + // Initialize the zenoh session. context->impl->session = z_open(z_move(config)); if (!z_session_check(&context->impl->session)) { @@ -130,9 +141,9 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) context->impl->graph_guard_condition, "failed to allocate graph guard condition", return RMW_RET_BAD_ALLOC); - auto cleanup_guard_condition = rcpputils::make_scope_exit( - [context]() { - rmw_guard_condition_free(context->impl->graph_guard_condition); + auto free_guard_condition = rcpputils::make_scope_exit( + [context, allocator]() { + allocator->deallocate(context->impl->graph_guard_condition, allocator->state); }); context->impl->graph_guard_condition->implementation_identifier = rmw_zenoh_identifier; @@ -148,18 +159,23 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); }); - new(context->impl->graph_guard_condition->data) GuardCondition; - auto destruct_guard_condition = rcpputils::make_scope_exit( + RMW_TRY_PLACEMENT_NEW( + context->impl->graph_guard_condition->data, + context->impl->graph_guard_condition->data, + return RMW_RET_BAD_ALLOC, + GuardCondition); + auto destruct_guard_condition_data = rcpputils::make_scope_exit( [context]() { auto gc_data = static_cast(context->impl->graph_guard_condition->data); RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(gc_data->~GuardCondition(), GuardCondition); }); - destruct_guard_condition.cancel(); + destruct_guard_condition_data.cancel(); free_guard_condition_data.cancel(); - cleanup_guard_condition.cancel(); + free_guard_condition.cancel(); close_session.cancel(); free_options.cancel(); + impl_destructor.cancel(); free_impl.cancel(); restore_context.cancel(); @@ -171,6 +187,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) rmw_ret_t rmw_shutdown(rmw_context_t * context) { + fprintf(stderr, "Shutting down context\n"); RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_FOR_NULL_WITH_MSG( context->impl, @@ -196,6 +213,7 @@ rmw_shutdown(rmw_context_t * context) rmw_ret_t rmw_context_fini(rmw_context_t * context) { + fprintf(stderr, "Finalizing context\n"); RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_FOR_NULL_WITH_MSG( context->impl, @@ -213,19 +231,18 @@ rmw_context_fini(rmw_context_t * context) const rcutils_allocator_t * allocator = &context->options.allocator; - static_cast(context->impl->graph_guard_condition->data)->~GuardCondition(); + RMW_TRY_DESTRUCTOR( + static_cast(context->impl->graph_guard_condition->data)->~GuardCondition(), + GuardCondition, ); allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); allocator->deallocate(context->impl->graph_guard_condition, allocator->state); - if (!context->impl->is_shutdown) { - z_close(z_move(context->impl->session)); - } + RMW_TRY_DESTRUCTOR(context->impl->~rmw_context_impl_t(), rmw_context_impl_t, ); + allocator->deallocate(context->impl, allocator->state); rmw_ret_t ret = rmw_init_options_fini(&context->options); - delete context->impl; - *context = rmw_get_zero_initialized_context(); return ret; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index acc69617..d821084e 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -15,6 +15,8 @@ #include #include +#include +#include #include #include "detail/guard_condition.hpp" @@ -96,6 +98,7 @@ rmw_create_node( RCUTILS_SET_ERROR_MSG("context has been shutdown"); return nullptr; } + int validation_result = RMW_NODE_NAME_VALID; rmw_ret_t ret = rmw_validate_node_name(name, &validation_result, nullptr); if (RMW_RET_OK != ret) { @@ -106,6 +109,7 @@ rmw_create_node( RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid node name: %s", reason); return nullptr; } + validation_result = RMW_NAMESPACE_VALID; ret = rmw_validate_namespace(namespace_, &validation_result, nullptr); if (RMW_RET_OK != ret) { @@ -125,7 +129,7 @@ rmw_create_node( node, "unable to allocate memory for rmw_node_t", return nullptr); - auto cleanup_node = rcpputils::make_scope_exit( + auto free_node = rcpputils::make_scope_exit( [node, allocator]() { allocator->deallocate(node, allocator->state); }); @@ -169,7 +173,7 @@ rmw_create_node( free_node_data.cancel(); free_namespace.cancel(); free_name.cancel(); - cleanup_node.cancel(); + free_node.cancel(); return node; } @@ -190,9 +194,9 @@ rmw_destroy_node(rmw_node_t * node) rcutils_allocator_t * allocator = &node->context->options.allocator; - allocator->deallocate(const_cast(node->name), allocator->state); - allocator->deallocate(const_cast(node->namespace_), allocator->state); allocator->deallocate(node->data, allocator->state); + allocator->deallocate(const_cast(node->namespace_), allocator->state); + allocator->deallocate(const_cast(node->name), allocator->state); allocator->deallocate(node, allocator->state); return RMW_RET_OK; @@ -209,7 +213,10 @@ rmw_node_get_graph_guard_condition(const rmw_node_t * node) node->implementation_identifier, rmw_zenoh_identifier, return nullptr); - // TODO(Yadunund): Also check if node->data is valid. + + RMW_CHECK_ARGUMENT_FOR_NULL(node->context, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, nullptr); + return node->context->impl->graph_guard_condition; } @@ -267,6 +274,31 @@ static char * ros_topic_name_to_zenoh_key( return rcutils_strndup(&topic_name[start_offset], end_offset - start_offset, *allocator); } +static const rosidl_message_type_support_t * find_type_support( + const rosidl_message_type_support_t * type_supports) +{ + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle(type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } + } + + return type_support; +} + //============================================================================== /// Create a publisher and return a handle to that publisher. rmw_publisher_t * @@ -319,23 +351,10 @@ rmw_create_publisher( } // Get the RMW type support. - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); - if (!type_support) { - rcutils_error_string_t prev_error_string = rcutils_get_error_string(); - rcutils_reset_error(); - type_support = get_message_typesupport_handle(type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); - if (!type_support) { - rcutils_error_string_t error_string = rcutils_get_error_string(); - rcutils_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Type support not from this implementation. Got:\n" - " %s\n" - " %s\n" - "while fetching it", - prev_error_string.str, error_string.str); - return nullptr; - } + const rosidl_message_type_support_t * type_support = find_type_support(type_supports); + if (type_support == nullptr) { + // error was already set by find_type_support + return nullptr; } RMW_CHECK_FOR_NULL_WITH_MSG( @@ -367,12 +386,6 @@ rmw_create_publisher( rmw_publisher, "failed to allocate memory for the publisher", return nullptr); - // Get typed pointer to implementation specific publisher data struct - // auto publisher_data = static_cast(rmw_publisher->data); - // if (publisher_data == nullptr) { - // RMW_SET_ERROR_MSG("unable to cast publisher data into rmw_publisher_data_t"); - // return nullptr; - // } auto free_rmw_publisher = rcpputils::make_scope_exit( [rmw_publisher, allocator]() { allocator->deallocate(rmw_publisher, allocator->state); @@ -426,11 +439,15 @@ rmw_create_publisher( allocator->deallocate(publisher_data->type_support, allocator->state); }); - new(publisher_data->type_support) MessageTypeSupport(callbacks); + RMW_TRY_PLACEMENT_NEW( + publisher_data->type_support, + publisher_data->type_support, + return nullptr, + MessageTypeSupport, callbacks); auto destruct_msg_type_support = rcpputils::make_scope_exit( [publisher_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - static_cast(publisher_data->type_support)->~MessageTypeSupport(), + publisher_data->type_support->~MessageTypeSupport(), MessageTypeSupport); }); @@ -440,9 +457,18 @@ rmw_create_publisher( rmw_publisher->options = *publisher_options; // TODO(yadunund): Update this. rmw_publisher->can_loan_messages = false; + rmw_publisher->topic_name = rcutils_strdup(topic_name, *allocator); - RMW_CHECK_ARGUMENT_FOR_NULL(rmw_publisher->topic_name, nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG( + rmw_publisher->topic_name, + "Failed to allocate topic name", + return nullptr); + auto free_topic_name = rcpputils::make_scope_exit( + [rmw_publisher, allocator]() { + allocator->deallocate(const_cast(rmw_publisher->topic_name), allocator->state); + }); + free_topic_name.cancel(); destruct_msg_type_support.cancel(); free_type_support.cancel(); undeclare_z_publisher.cancel(); @@ -478,6 +504,7 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) auto publisher_data = static_cast(publisher->data); if (publisher_data != nullptr) { + RMW_TRY_DESTRUCTOR(publisher_data->type_support->~MessageTypeSupport(), MessageTypeSupport, ); allocator->deallocate(publisher_data->type_support, allocator->state); if (z_undeclare_publisher(z_move(publisher_data->pub))) { RMW_SET_ERROR_MSG("failed to undeclare pub"); @@ -664,6 +691,7 @@ rmw_publisher_count_matched_subscriptions( static_cast(publisher); static_cast(subscription_count); // TODO(yadunund): Fixme. + *subscription_count = 0; return RMW_RET_OK; } @@ -841,8 +869,8 @@ rmw_create_subscription( return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); - if (0 == strlen(topic_name)) { - RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); + if (topic_name[0] == '\0') { + RMW_SET_ERROR_MSG("topic_name argument is an empty string"); return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(qos_profile, nullptr); @@ -861,25 +889,12 @@ rmw_create_subscription( // return nullptr; // } - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); - if (!type_support) { - rcutils_error_string_t prev_error_string = rcutils_get_error_string(); - rcutils_reset_error(); - type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); - if (!type_support) { - rcutils_error_string_t error_string = rcutils_get_error_string(); - rcutils_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Type support not from this implementation. Got:\n" - " %s\n" - " %s\n" - "while fetching it", - prev_error_string.str, error_string.str); - return nullptr; - } + const rosidl_message_type_support_t * type_support = find_type_support(type_supports); + if (type_support == nullptr) { + // error was already set by find_type_support + return nullptr; } + auto node_data = static_cast(node->data); RMW_CHECK_FOR_NULL_WITH_MSG( node_data, "unable to create subscription as node_data is invalid.", @@ -905,30 +920,40 @@ rmw_create_subscription( return nullptr; } + rcutils_allocator_t * allocator = &node->context->options.allocator; + // Create the rmw_subscription. - rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); - return nullptr; - } + rmw_subscription_t * rmw_subscription = + static_cast(allocator->zero_allocate( + 1, sizeof(rmw_subscription_t), allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + rmw_subscription, + "failed to allocate memory for the subscription", + return nullptr); auto free_rmw_subscription = rcpputils::make_scope_exit( - [rmw_subscription]() { - rmw_subscription_free(rmw_subscription); + [rmw_subscription, allocator]() { + allocator->deallocate(rmw_subscription, allocator->state); }); - rcutils_allocator_t * allocator = &node->context->options.allocator; - auto * sub_data = - static_cast(allocator->allocate( - sizeof(rmw_subscription_data_t), - allocator->state)); - if (sub_data == nullptr) { - RMW_SET_ERROR_MSG("failed to allocate sub_data"); - return nullptr; - } + + auto sub_data = static_cast( + allocator->allocate(sizeof(rmw_subscription_data_t), allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + sub_data, + "failed to allocate memory for subscription data", + return nullptr); auto free_sub_data = rcpputils::make_scope_exit( [sub_data, allocator]() { allocator->deallocate(sub_data, allocator->state); }); + RMW_TRY_PLACEMENT_NEW(sub_data, sub_data, return nullptr, rmw_subscription_data_t); + auto destruct_sub_data = rcpputils::make_scope_exit( + [sub_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + sub_data->~rmw_subscription_data_t(), + rmw_subscription_data_t); + }); + // Set the reliability of the subscription options based on qos_profile. // The default options will be initialized with Best Effort reliability. auto sub_options = z_subscriber_options_default(); @@ -938,28 +963,11 @@ rmw_create_subscription( sub_data->reliable = true; } - z_owned_closure_sample_t callback = z_closure(sub_data_handler, nullptr, sub_data); - sub_data->sub = z_declare_subscriber( - z_loan(context_impl->session), - z_keyexpr(&topic_name[1]), - z_move(callback), - &sub_options - ); - if (!z_check(sub_data->sub)) { - RMW_SET_ERROR_MSG("unable to create zenoh subscription"); - return nullptr; - } - auto undeclare_z_sub = rcpputils::make_scope_exit( - [sub_data]() { - z_undeclare_subscriber(z_move(sub_data->sub)); - }); - - sub_data->type_support_impl = type_support->data; sub_data->typesupport_identifier = type_support->typesupport_identifier; + sub_data->type_support_impl = type_support->data; auto callbacks = static_cast(type_support->data); - // std::string type_name = _create_type_name(callbacks); sub_data->type_support = static_cast( - rmw_allocate(sizeof(MessageTypeSupport))); + allocator->allocate(sizeof(MessageTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( sub_data->type_support, "Failed to allocate MessageTypeSupport", @@ -969,25 +977,72 @@ rmw_create_subscription( allocator->deallocate(sub_data->type_support, allocator->state); }); + RMW_TRY_PLACEMENT_NEW( + sub_data->type_support, + sub_data->type_support, + return nullptr, + MessageTypeSupport, callbacks); + auto destruct_msg_type_support = rcpputils::make_scope_exit( + [sub_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + sub_data->type_support->~MessageTypeSupport(), + MessageTypeSupport); + }); + sub_data->queue_depth = qos_profile->depth; - new(sub_data->type_support) MessageTypeSupport(callbacks); + sub_data->context = node->context; + rmw_subscription->implementation_identifier = rmw_zenoh_identifier; rmw_subscription->data = sub_data; + rmw_subscription->topic_name = rcutils_strdup(topic_name, *allocator); - RMW_CHECK_ARGUMENT_FOR_NULL(rmw_subscription->topic_name, nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG( + rmw_subscription->topic_name, + "Failed to allocate topic name", + return nullptr); auto free_topic_name = rcpputils::make_scope_exit( [rmw_subscription, allocator]() { allocator->deallocate(const_cast(rmw_subscription->topic_name), allocator->state); }); + rmw_subscription->options = *subscription_options; rmw_subscription->can_loan_messages = false; rmw_subscription->is_cft_enabled = false; + // Everything above succeeded and is setup properly. Now declare a subscriber + // with Zenoh; after this, callbacks may come in at any time. + + z_owned_closure_sample_t callback = z_closure(sub_data_handler, nullptr, sub_data); + char * zenoh_key_name = ros_topic_name_to_zenoh_key(topic_name, allocator); + // TODO(clalancette): What happens if the key name is a valid but empty string? + RMW_CHECK_FOR_NULL_WITH_MSG( + zenoh_key_name, + "failed to allocate memory for zenoh key name", + return nullptr); + sub_data->sub = z_declare_subscriber( + z_loan(context_impl->session), + z_keyexpr(zenoh_key_name), + z_move(callback), + &sub_options + ); + allocator->deallocate(zenoh_key_name, allocator->state); + if (!z_check(sub_data->sub)) { + RMW_SET_ERROR_MSG("unable to create zenoh subscription"); + return nullptr; + } + auto undeclare_z_sub = rcpputils::make_scope_exit( + [sub_data]() { + z_undeclare_subscriber(z_move(sub_data->sub)); + }); + + undeclare_z_sub.cancel(); free_topic_name.cancel(); + destruct_msg_type_support.cancel(); free_type_support.cancel(); - undeclare_z_sub.cancel(); + destruct_sub_data.cancel(); free_sub_data.cancel(); free_rmw_subscription.cancel(); + return rmw_subscription; } @@ -1011,19 +1066,25 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) rmw_ret_t ret = RMW_RET_OK; - rmw_free(const_cast(subscription->topic_name)); + rcutils_allocator_t * allocator = &node->context->options.allocator; + + allocator->deallocate(const_cast(subscription->topic_name), allocator->state); auto sub_data = static_cast(subscription->data); if (sub_data != nullptr) { - rcutils_allocator_t * allocator = &node->context->options.allocator; + RMW_TRY_DESTRUCTOR(sub_data->type_support->~MessageTypeSupport(), MessageTypeSupport, ); allocator->deallocate(sub_data->type_support, allocator->state); + if (z_undeclare_subscriber(z_move(sub_data->sub))) { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; } + + RMW_TRY_DESTRUCTOR(sub_data->~rmw_subscription_data_t(), rmw_subscription_data_t, ); allocator->deallocate(sub_data, allocator->state); } - rmw_subscription_free(subscription); + allocator->deallocate(subscription, allocator->state); + return ret; } @@ -1035,7 +1096,10 @@ rmw_subscription_count_matched_publishers( size_t * publisher_count) { static_cast(subscription); - static_cast(publisher_count); + + // TODO(clalancette): implement + *publisher_count = 0; + return RMW_RET_UNSUPPORTED; } @@ -1088,26 +1152,7 @@ rmw_subscription_get_content_filter( return RMW_RET_UNSUPPORTED; } -//============================================================================== -/// Take an incoming ROS message. -rmw_ret_t -rmw_take( - const rmw_subscription_t * subscription, - void * ros_message, - bool * taken, - rmw_subscription_allocation_t * allocation) -{ - static_cast(subscription); - static_cast(ros_message); - static_cast(taken); - static_cast(allocation); - return RMW_RET_UNSUPPORTED; -} - -//============================================================================== -/// Take an incoming ROS message with its metadata. -rmw_ret_t -rmw_take_with_info( +static rmw_ret_t __rmw_take( const rmw_subscription_t * subscription, void * ros_message, bool * taken, @@ -1126,31 +1171,31 @@ rmw_take_with_info( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); static_cast(allocation); + *taken = false; - auto * sub_data = static_cast(subscription->data); + auto sub_data = static_cast(subscription->data); // RETRIEVE SERIALIZED MESSAGE =============================================== - std::unique_lock lock(sub_data->message_queue_mutex); - if (sub_data->message_queue.empty()) { - // This tells rcl that the check for a new message was done, but no messages have come in yet. - return RMW_RET_OK; - } + std::pair msg_bytes; + { + std::unique_lock lock(sub_data->message_queue_mutex); - // NOTE(CH3): Potential place to handle "QoS" (e.g. could pop from back so it is LIFO) - auto msg_bytes_ptr = sub_data->message_queue.back(); - sub_data->message_queue.pop_back(); - sub_data->message_queue_mutex.unlock(); + if (sub_data->message_queue.empty()) { + // This tells rcl that the check for a new message was done, but no messages have come in yet. + return RMW_RET_OK; + } - unsigned char * cdr_buffer = static_cast(rmw_allocate( - msg_bytes_ptr->size())); - memcpy(cdr_buffer, &msg_bytes_ptr->front(), msg_bytes_ptr->size()); + // NOTE(CH3): Potential place to handle "QoS" (e.g. could pop from back so it is LIFO) + msg_bytes = sub_data->message_queue.back(); + sub_data->message_queue.pop_back(); + } // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(cdr_buffer), - msg_bytes_ptr->size()); + reinterpret_cast(msg_bytes.second), + msg_bytes.first); // Object that serializes the data eprosima::fastcdr::Cdr deser( @@ -1166,12 +1211,49 @@ rmw_take_with_info( return RMW_RET_ERROR; } + rcutils_allocator_t * allocator = &sub_data->context->options.allocator; *taken = true; - rmw_free(cdr_buffer); + allocator->deallocate(msg_bytes.second, allocator->state); + + // TODO(clalancette): fill in message_info here + message_info->source_timestamp = 0; + message_info->received_timestamp = 0; + message_info->publication_sequence_number = 0; + message_info->reception_sequence_number = 0; + message_info->publisher_gid.implementation_identifier = rmw_zenoh_identifier; + memset(message_info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); + message_info->from_intra_process = false; return RMW_RET_OK; } +//============================================================================== +/// Take an incoming ROS message. +rmw_ret_t +rmw_take( + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + rmw_message_info_t dummy_msg_info; + + return __rmw_take(subscription, ros_message, taken, &dummy_msg_info, allocation); +} + +//============================================================================== +/// Take an incoming ROS message with its metadata. +rmw_ret_t +rmw_take_with_info( + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + return __rmw_take(subscription, ros_message, taken, message_info, allocation); +} + //============================================================================== /// Take multiple incoming ROS messages with their metadata. rmw_ret_t @@ -1568,26 +1650,29 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) rmw_zenoh_identifier, return nullptr); - rmw_wait_set_t * wait_set = rmw_wait_set_allocate(); - if (!wait_set) { - RMW_SET_ERROR_MSG("failed to allocate wait set"); - return nullptr; - } + rcutils_allocator_t * allocator = &context->options.allocator; + + auto wait_set = static_cast( + allocator->zero_allocate(1, sizeof(rmw_wait_set_t), allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + wait_set, + "failed to allocate wait set", + return nullptr); auto cleanup_wait_set = rcpputils::make_scope_exit( - [wait_set]() { - rmw_wait_set_free(wait_set); + [wait_set, allocator]() { + allocator->deallocate(wait_set, allocator->state); }); wait_set->implementation_identifier = rmw_zenoh_identifier; - wait_set->data = rmw_allocate(sizeof(rmw_wait_set_data_t)); - if (!wait_set->data) { - RMW_SET_ERROR_MSG("failed to construct wait set info struct"); - return nullptr; - } + wait_set->data = allocator->zero_allocate(1, sizeof(rmw_wait_set_data_t), allocator->state); + RMW_CHECK_FOR_NULL_WITH_MSG( + wait_set->data, + "failed to allocate wait set data", + return nullptr); auto free_wait_set_data = rcpputils::make_scope_exit( - [wait_set]() { - rmw_free(wait_set->data); + [wait_set, allocator]() { + allocator->deallocate(wait_set->data, allocator->state); }); // Invoke placement new @@ -1599,9 +1684,12 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) rmw_wait_set_data); }); + static_cast(wait_set->data)->context = context; + destruct_rmw_wait_set_data.cancel(); free_wait_set_data.cancel(); cleanup_wait_set.cancel(); + return wait_set; } @@ -1620,111 +1708,16 @@ rmw_destroy_wait_set(rmw_wait_set_t * wait_set) auto wait_set_data = static_cast(wait_set->data); + rcutils_allocator_t * allocator = &wait_set_data->context->options.allocator; + wait_set_data->~rmw_wait_set_data_t(); - rmw_free(wait_set->data); + allocator->deallocate(wait_set_data, allocator->state); - rmw_wait_set_free(wait_set); + allocator->deallocate(wait_set, allocator->state); return RMW_RET_OK; } -//============================================================================== -namespace -{ -bool check_wait_conditions( - const rmw_subscriptions_t * subscriptions, - const rmw_guard_conditions_t * guard_conditions, - const rmw_services_t * services, - const rmw_clients_t * clients, - const rmw_events_t * events, - bool finalize) -{ - // NOTE(CH3): On the finalize parameter - // This check function is used as a predicate to wait on a condition variable. But rcl expects - // rmw to set any passed in pointers to NULL if the condition is not ready. - // - // The finalize parameter is used to make sure that this setting to NULL only happens ONCE per - // rmw_wait call. Otherwise on repeat calls to check the predicate, things will break since - // it'll try to compare or dereference a nullptr. - - bool stop_wait = false; - - // SUBSCRIPTIONS ============================================================= - if (subscriptions) { - size_t subscriptions_ready = 0; - - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - auto subscription_data = static_cast( - subscriptions->subscribers[i]); - if (subscription_data && subscription_data->message_queue.empty()) { - if (finalize) { - // Setting to nullptr lets rcl know that this subscription is not ready - subscriptions->subscribers[i] = nullptr; - } - } else { - subscriptions_ready++; - stop_wait = true; - } - } - - if (finalize && subscriptions_ready > 0) { - RCUTILS_LOG_DEBUG_NAMED( - "rmw_zenoh_cpp", "[rmw_wait] SUBSCRIPTIONS READY: %ld", - subscriptions_ready); - } - } - - // SERVICES ================================================================== - // TODO(yadunund) - for (size_t i = 0; i < services->service_count; ++i) { - services->services[i] = nullptr; - } - - // CLIENTS =================================================================== - // TODO(yadunund) - for (size_t i = 0; i < clients->client_count; ++i) { - clients->clients[i] = nullptr; - } - - // Guard conditions - // for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - // guard_conditions->guard_conditions[i] = nullptr; - // } - if (guard_conditions) { - size_t guard_conditions_ready = 0; - - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - auto guard_condition = static_cast( - guard_conditions->guard_conditions[i]); - if (guard_condition && guard_condition->has_triggered()) { - if (finalize) { - // Setting to nullptr lets rcl know that this guard_condition is not ready - guard_conditions->guard_conditions[i] = nullptr; - } - } else { - guard_conditions_ready++; - stop_wait = true; - } - } - - if (finalize && guard_conditions_ready > 0) { - RCUTILS_LOG_DEBUG_NAMED( - "rmw_zenoh_common_cpp", "[rmw_wait] GUARD CONDITIONS READY: %ld", - guard_conditions_ready); - } - } - - // Events - // TODO(yadunund) - for (size_t i = 0; i < events->event_count; ++i) { - events->events[i] = nullptr; - } - - return stop_wait; -} - -} // namespace - //============================================================================== /// Waits on sets of different entities and returns when one is ready. rmw_ret_t @@ -1743,7 +1736,8 @@ rmw_wait( wait_set->implementation_identifier, rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RCUTILS_LOG_DEBUG_NAMED( + // TODO(yadunund): Switch to debug log level. + RCUTILS_LOG_WARN_NAMED( "rmw_zenoh_cpp", "[rmw_wait] %ld subscriptions, %ld services, %ld clients, %ld events, %ld guard conditions", subscriptions->subscriber_count, @@ -1752,83 +1746,91 @@ rmw_wait( events->event_count, guard_conditions->guard_condition_count); + // TODO(yadunund): Switch to debug log level. if (wait_timeout) { - RCUTILS_LOG_DEBUG_NAMED( + RCUTILS_LOG_WARN_NAMED( "rmw_zenoh_common_cpp", "[rmw_wait] TIMEOUT: %ld s %ld ns", wait_timeout->sec, wait_timeout->nsec); } - auto wait_set_info = static_cast(wait_set->data); - if (!wait_set_info) { - RMW_SET_ERROR_MSG("Waitset info struct is null"); - return RMW_RET_ERROR; + auto wait_set_data = static_cast(wait_set->data); + RMW_CHECK_FOR_NULL_WITH_MSG( + wait_set_data, + "waitset data struct is null", + return RMW_RET_ERROR); + + if (guard_conditions) { + // Go through each of the guard conditions, and attach the wait set condition variable to them. + // That way they can wake it up if they are triggered while we are waiting. + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + // This is hard to track down, but each of the (void *) pointers in + // guard_conditions->guard_conditions points to the data field of the related + // rmw_guard_condition_t. So we can directly cast it to GuardCondition. + GuardCondition * gc = static_cast(guard_conditions->guard_conditions[i]); + if (gc != nullptr) { + gc->attach_condition(&wait_set_data->condition_variable); + } + } } - std::mutex * condition_mutex = &wait_set_info->condition_mutex; - if (!condition_mutex) { - RMW_SET_ERROR_MSG("Mutex for wait set was null"); - return RMW_RET_ERROR; + if (subscriptions) { + // Go through each of the subscriptions and attach the wait set condition variable to them. + // That way they can wake it up if they are triggered while we are waiting. + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + auto sub_data = static_cast(subscriptions->subscribers[i]); + if (sub_data != nullptr) { + sub_data->condition = &wait_set_data->condition_variable; + } + } } - std::condition_variable * condition_variable = &wait_set_info->condition; - if (!condition_variable) { - RMW_SET_ERROR_MSG("Condition variable for wait set was null"); - return RMW_RET_ERROR; + std::unique_lock lock(wait_set_data->condition_mutex); + + // According to the RMW documentation, if wait_timeout is NULL that means + // "wait forever", if it specified by 0 it means "never wait", and if it is anything else wait + // for that amount of time. + if (wait_timeout == nullptr) { + wait_set_data->condition_variable.wait(lock); + } else { + if (wait_timeout->sec != 0 || wait_timeout->nsec != 0) { + wait_set_data->condition_variable.wait_for( + lock, std::chrono::nanoseconds(wait_timeout->nsec + RCUTILS_S_TO_NS(wait_timeout->sec))); + } } - std::unique_lock lock(*condition_mutex); - - bool ready = check_wait_conditions( - subscriptions, - guard_conditions, - services, - clients, - events, - false); - auto predicate = [subscriptions, guard_conditions, services, clients, events]() { - return check_wait_conditions( - subscriptions, - guard_conditions, - services, - clients, - events, - false); - }; - - bool timed_out = false; - - if (!ready) { - if (!wait_timeout) { - // TODO(CH3): Remove this magic number once stable. This is to slow things down so things are - // visible with all the printouts flying everywhere. - condition_variable->wait_for(lock, std::chrono::milliseconds(500), predicate); - } else if (wait_timeout->sec > 0 || wait_timeout->nsec > 0) { - auto wait_time = std::chrono::duration_cast( - std::chrono::seconds(wait_timeout->sec)); - wait_time += std::chrono::nanoseconds(wait_timeout->nsec); - - timed_out = !condition_variable->wait_for(lock, wait_time, predicate); - } else { - timed_out = true; + if (guard_conditions) { + // Now detach the condition variable and mutex from each of the guard conditions + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + GuardCondition * gc = static_cast(guard_conditions->guard_conditions[i]); + if (gc != nullptr) { + gc->detach_condition(); + // According to the documentation for rmw_wait in rmw.h, entries in the + // array that have *not* been triggered should be set to NULL + if (!gc->has_triggered()) { + guard_conditions->guard_conditions[i] = nullptr; + } + } } } - // The finalize parameter passed in here enables debug and setting of non-ready conditions - // to NULL (as expected by rcl) - // - // (In other words, it ensures that this happens once per rmw_wait call) - // - // Debug logs and NULL assignments do not happen in the predicate above, and only on this call - check_wait_conditions(subscriptions, guard_conditions, services, clients, events, true); - lock.unlock(); - - if (timed_out) { - RCUTILS_LOG_DEBUG_NAMED("rmw_zenoh_common_cpp", "[rmw_wait] TIMED OUT"); - return RMW_RET_TIMEOUT; - } else { - return RMW_RET_OK; + if (subscriptions) { + // Now detach the condition variable and mutex from each of the subscriptions + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + auto sub_data = static_cast(subscriptions->subscribers[i]); + if (sub_data != nullptr) { + sub_data->condition = nullptr; + // According to the documentation for rmw_wait in rmw.h, entries in the + // array that have *not* been triggered should be set to NULL + if (sub_data->message_queue.empty()) { + // Setting to nullptr lets rcl know that this subscription is not ready + subscriptions->subscribers[i] = nullptr; + } + } + } } + + return RMW_RET_OK; } //============================================================================== @@ -1924,7 +1926,8 @@ rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) { static_cast(publisher); static_cast(gid); - return RMW_RET_UNSUPPORTED; + // TODO(clalancette): Implement me + return RMW_RET_OK; } //============================================================================== From 3bf5de3df20329df73dae585d6991336b7c72970 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 5 Oct 2023 08:14:05 -0400 Subject: [PATCH 35/50] Implement msg_info (#21) * Introduce saved_msg_data struct. We save off the message data as returned by the sub_data_handler. * Fill in more of the message info. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 8 +++++--- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 16 +++++++++++++++- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 18 ++++++++++-------- 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 40e96e10..ea421542 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -66,12 +66,14 @@ void sub_data_handler( sub_data->queue_depth, z_loan(keystr)); - std::pair old = sub_data->message_queue.back(); - allocator->deallocate(old.second, allocator->state); + std::unique_ptr old = std::move(sub_data->message_queue.back()); + allocator->deallocate(old->data, allocator->state); sub_data->message_queue.pop_back(); } - sub_data->message_queue.push_front(std::make_pair(sample->payload.len, cdr_buffer)); + sub_data->message_queue.emplace_front( + std::make_unique( + sample->payload.len, cdr_buffer, sample->timestamp.time, sample->timestamp.id.id)); // Since we added new data, trigger the guard condition if it is available std::lock_guard internal_lock(sub_data->internal_mutex); diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 3e91c421..b9bc99da 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -85,6 +85,20 @@ struct rmw_wait_set_data_t // z_owned_closure_sample_t void sub_data_handler(const z_sample_t * sample, void * sub_data); +struct saved_msg_data +{ + explicit saved_msg_data(size_t len, uint8_t * d, uint64_t recv_ts, const uint8_t pub_gid[16]) + : data_length(len), data(d), recv_timestamp(recv_ts) + { + memcpy(publisher_gid, pub_gid, 16); + } + + size_t data_length; + uint8_t * data; + uint64_t recv_timestamp; + uint8_t publisher_gid[16]; +}; + ///============================================================================== struct rmw_subscription_data_t { @@ -95,7 +109,7 @@ struct rmw_subscription_data_t MessageTypeSupport * type_support; rmw_context_t * context; - std::deque> message_queue; + std::deque> message_queue; std::mutex message_queue_mutex; size_t queue_depth; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index d821084e..0db1f3c2 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1178,7 +1178,7 @@ static rmw_ret_t __rmw_take( // RETRIEVE SERIALIZED MESSAGE =============================================== - std::pair msg_bytes; + std::unique_ptr msg_data; { std::unique_lock lock(sub_data->message_queue_mutex); @@ -1188,14 +1188,14 @@ static rmw_ret_t __rmw_take( } // NOTE(CH3): Potential place to handle "QoS" (e.g. could pop from back so it is LIFO) - msg_bytes = sub_data->message_queue.back(); + msg_data = std::move(sub_data->message_queue.back()); sub_data->message_queue.pop_back(); } // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(msg_bytes.second), - msg_bytes.first); + reinterpret_cast(msg_data->data), + msg_data->data_length); // Object that serializes the data eprosima::fastcdr::Cdr deser( @@ -1213,15 +1213,17 @@ static rmw_ret_t __rmw_take( rcutils_allocator_t * allocator = &sub_data->context->options.allocator; *taken = true; - allocator->deallocate(msg_bytes.second, allocator->state); + allocator->deallocate(msg_data->data, allocator->state); - // TODO(clalancette): fill in message_info here + // TODO(clalancette): fill in source_timestamp message_info->source_timestamp = 0; - message_info->received_timestamp = 0; + message_info->received_timestamp = msg_data->recv_timestamp; + // TODO(clalancette): fill in publication_sequence_number message_info->publication_sequence_number = 0; + // TODO(clalancette): fill in reception_sequence_number message_info->reception_sequence_number = 0; message_info->publisher_gid.implementation_identifier = rmw_zenoh_identifier; - memset(message_info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); + memcpy(message_info->publisher_gid.data, msg_data->publisher_gid, 16); message_info->from_intra_process = false; return RMW_RET_OK; From 1825ff69aecfbb6eba5436986e8976c75717b646 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 5 Oct 2023 08:25:26 -0400 Subject: [PATCH 36/50] Implement rmw_serialize and rmw_deserialize. (#22) Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 49 +++++++++++++++++++++++++++------ 1 file changed, 41 insertions(+), 8 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 0db1f3c2..33d395fe 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -21,6 +21,7 @@ #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" +#include "detail/MessageTypeSupport.hpp" #include "detail/rmw_data_types.hpp" #include "detail/serialization_format.hpp" #include "detail/type_support_common.hpp" @@ -806,10 +807,31 @@ rmw_serialize( const rosidl_message_type_support_t * type_support, rmw_serialized_message_t * serialized_message) { - static_cast(ros_message); - static_cast(type_support); - static_cast(serialized_message); - return RMW_RET_UNSUPPORTED; + const rosidl_message_type_support_t * ts = find_type_support(type_support); + if (ts == nullptr) { + // error was already set by find_type_support + return RMW_RET_ERROR; + } + + auto callbacks = static_cast(ts->data); + auto tss = MessageTypeSupport(callbacks); + auto data_length = tss.getEstimatedSerializedSize(ros_message, callbacks); + if (serialized_message->buffer_capacity < data_length) { + if (rmw_serialized_message_resize(serialized_message, data_length) != RMW_RET_OK) { + RMW_SET_ERROR_MSG("unable to dynamically resize serialized message"); + return RMW_RET_ERROR; + } + } + + eprosima::fastcdr::FastBuffer buffer( + reinterpret_cast(serialized_message->buffer), data_length); + eprosima::fastcdr::Cdr ser( + buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + + auto ret = tss.serializeROSmessage(ros_message, ser, callbacks); + serialized_message->buffer_length = data_length; + serialized_message->buffer_capacity = data_length; + return ret == true ? RMW_RET_OK : RMW_RET_ERROR; } //============================================================================== @@ -820,10 +842,21 @@ rmw_deserialize( const rosidl_message_type_support_t * type_support, void * ros_message) { - static_cast(serialized_message); - static_cast(type_support); - static_cast(ros_message); - return RMW_RET_UNSUPPORTED; + const rosidl_message_type_support_t * ts = find_type_support(type_support); + if (ts == nullptr) { + // error was already set by find_type_support + return RMW_RET_ERROR; + } + + auto callbacks = static_cast(ts->data); + auto tss = MessageTypeSupport(callbacks); + eprosima::fastcdr::FastBuffer buffer( + reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); + eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + + auto ret = tss.deserializeROSmessage(deser, ros_message, callbacks); + return ret == true ? RMW_RET_OK : RMW_RET_ERROR; } //============================================================================== From 24948712ef2219b611b5d814f18e75de3ecb8eb0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 5 Oct 2023 08:36:01 -0400 Subject: [PATCH 37/50] Rename a bunch of files (#23) * Rename MessageTypeSupport.hpp -> message_type_support.hpp * Rename ServiceTypeSupport.hpp -> service_type_support.hpp * Rename TypeSupport.hpp -> type_support.hpp That just makes it consistent with everything else. Signed-off-by: Chris Lalancette --- ...sageTypeSupport.hpp => message_type_support.hpp} | 8 ++++---- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 2 +- ...viceTypeSupport.hpp => service_type_support.hpp} | 9 ++++----- .../detail/{TypeSupport.hpp => type_support.hpp} | 13 +++---------- rmw_zenoh_cpp/src/detail/type_support_common.hpp | 6 +++--- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 2 +- 6 files changed, 16 insertions(+), 24 deletions(-) rename rmw_zenoh_cpp/src/detail/{MessageTypeSupport.hpp => message_type_support.hpp} (88%) rename rmw_zenoh_cpp/src/detail/{ServiceTypeSupport.hpp => service_type_support.hpp} (86%) rename rmw_zenoh_cpp/src/detail/{TypeSupport.hpp => type_support.hpp} (84%) diff --git a/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp b/rmw_zenoh_cpp/src/detail/message_type_support.hpp similarity index 88% rename from rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp rename to rmw_zenoh_cpp/src/detail/message_type_support.hpp index 5b2a810c..87a16780 100644 --- a/rmw_zenoh_cpp/src/detail/MessageTypeSupport.hpp +++ b/rmw_zenoh_cpp/src/detail/message_type_support.hpp @@ -16,11 +16,11 @@ // This file is originally from: // https://github.com/ros2/rmw_fastrtps/blob/b13e134cea2852aba210299bef6f4df172d9a0e3/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp -#ifndef DETAIL__MESSAGETYPESUPPORT_HPP_ -#define DETAIL__MESSAGETYPESUPPORT_HPP_ +#ifndef DETAIL__MESSAGE_TYPE_SUPPORT_HPP_ +#define DETAIL__MESSAGE_TYPE_SUPPORT_HPP_ #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" -#include "TypeSupport.hpp" +#include "type_support.hpp" ///============================================================================== class MessageTypeSupport : public TypeSupport @@ -29,4 +29,4 @@ class MessageTypeSupport : public TypeSupport explicit MessageTypeSupport(const message_type_support_callbacks_t * members); }; -#endif // DETAIL__MESSAGETYPESUPPORT_HPP_ +#endif // DETAIL__MESSAGE_TYPE_SUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index b9bc99da..3346ab8f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -29,7 +29,7 @@ #include "rmw/rmw.h" -#include "MessageTypeSupport.hpp" +#include "message_type_support.hpp" /// Structs for various type erased data fields. diff --git a/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp b/rmw_zenoh_cpp/src/detail/service_type_support.hpp similarity index 86% rename from rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp rename to rmw_zenoh_cpp/src/detail/service_type_support.hpp index 2dc6291d..48a8776d 100644 --- a/rmw_zenoh_cpp/src/detail/ServiceTypeSupport.hpp +++ b/rmw_zenoh_cpp/src/detail/service_type_support.hpp @@ -16,12 +16,11 @@ // This file is originally from: // https://github.com/ros2/rmw_fastrtps/blob/b13e134cea2852aba210299bef6f4df172d9a0e3/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp -#ifndef DETAIL__SERVICETYPESUPPORT_HPP_ -#define DETAIL__SERVICETYPESUPPORT_HPP_ +#ifndef DETAIL__SERVICE_TYPE_SUPPORT_HPP_ +#define DETAIL__SERVICE_TYPE_SUPPORT_HPP_ -#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" #include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" -#include "TypeSupport.hpp" +#include "type_support.hpp" ///============================================================================== class ServiceTypeSupport : public TypeSupport @@ -42,4 +41,4 @@ class ResponseTypeSupport : public ServiceTypeSupport explicit ResponseTypeSupport(const service_type_support_callbacks_t * members); }; -#endif // DETAIL__SERVICETYPESUPPORT_HPP_ +#endif // DETAIL__SERVICE_TYPE_SUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/TypeSupport.hpp b/rmw_zenoh_cpp/src/detail/type_support.hpp similarity index 84% rename from rmw_zenoh_cpp/src/detail/TypeSupport.hpp rename to rmw_zenoh_cpp/src/detail/type_support.hpp index 3a9df353..caa15ecf 100644 --- a/rmw_zenoh_cpp/src/detail/TypeSupport.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support.hpp @@ -16,22 +16,15 @@ // This file is originally from: // https://github.com/ros2/rmw_fastrtps/blob/b13e134cea2852aba210299bef6f4df172d9a0e3/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp -#ifndef DETAIL__TYPESUPPORT_HPP_ -#define DETAIL__TYPESUPPORT_HPP_ +#ifndef DETAIL__TYPE_SUPPORT_HPP_ +#define DETAIL__TYPE_SUPPORT_HPP_ -// #include - -#include #include -#include -#include #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" - -///============================================================================== class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport { public: @@ -53,4 +46,4 @@ class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport bool has_data_; }; -#endif // DETAIL__TYPESUPPORT_HPP_ +#endif // DETAIL__TYPE_SUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.hpp b/rmw_zenoh_cpp/src/detail/type_support_common.hpp index 431dc27b..712de5d5 100644 --- a/rmw_zenoh_cpp/src/detail/type_support_common.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support_common.hpp @@ -24,9 +24,9 @@ #include "rmw/error_handling.h" -#include "TypeSupport.hpp" -#include "MessageTypeSupport.hpp" -#include "ServiceTypeSupport.hpp" +#include "type_support.hpp" +#include "message_type_support.hpp" +#include "service_type_support.hpp" #include "identifier.hpp" diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 33d395fe..87afb79b 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -21,7 +21,7 @@ #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" -#include "detail/MessageTypeSupport.hpp" +#include "detail/message_type_support.hpp" #include "detail/rmw_data_types.hpp" #include "detail/serialization_format.hpp" #include "detail/type_support_common.hpp" From caacbd1ddaafec8a2b35d3b372d54b8f67af3bb5 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 5 Oct 2023 08:47:37 -0400 Subject: [PATCH 38/50] Move implementations around (#24) * Move MessageTypeSupport implementation to message_type_support.cpp * Add in a service_type_support.cpp file. * Rename type_support_common.cpp -> type_support.cpp * Split type_support_common implementation to a different file. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 3 + .../src/detail/message_type_support.cpp | 34 ++++ .../src/detail/service_type_support.cpp | 52 ++++++ rmw_zenoh_cpp/src/detail/type_support.cpp | 126 ++++++++++++++ .../src/detail/type_support_common.cpp | 162 ++---------------- .../src/detail/type_support_common.hpp | 35 +--- 6 files changed, 234 insertions(+), 178 deletions(-) create mode 100644 rmw_zenoh_cpp/src/detail/message_type_support.cpp create mode 100644 rmw_zenoh_cpp/src/detail/service_type_support.cpp create mode 100644 rmw_zenoh_cpp/src/detail/type_support.cpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 6160b316..713dd44f 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -29,7 +29,10 @@ find_package(zenohc REQUIRED) add_library(rmw_zenoh_cpp SHARED src/detail/identifier.cpp src/detail/guard_condition.cpp + src/detail/message_type_support.cpp src/detail/rmw_data_types.cpp + src/detail/service_type_support.cpp + src/detail/type_support.cpp src/detail/type_support_common.cpp src/rmw_event.cpp src/rmw_get_network_flow_endpoints.cpp diff --git a/rmw_zenoh_cpp/src/detail/message_type_support.cpp b/rmw_zenoh_cpp/src/detail/message_type_support.cpp new file mode 100644 index 00000000..861d0db7 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/message_type_support.cpp @@ -0,0 +1,34 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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. + +// Parts of this file are originally from: +// https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp + +#include + +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" + +#include "message_type_support.hpp" +#include "type_support_common.hpp" + +MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * members) +{ + assert(members); + + std::string name = _create_type_name(members); + this->setName(name.c_str()); + + set_members(members); +} diff --git a/rmw_zenoh_cpp/src/detail/service_type_support.cpp b/rmw_zenoh_cpp/src/detail/service_type_support.cpp new file mode 100644 index 00000000..6b471715 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/service_type_support.cpp @@ -0,0 +1,52 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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. + +// Parts of this file are originally from: +// https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp + +#include + +#include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" + +#include "service_type_support.hpp" +#include "type_support_common.hpp" + +ServiceTypeSupport::ServiceTypeSupport() +{ +} + +RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * members) +{ + assert(members); + + auto msg = static_cast( + members->request_members_->data); + std::string name = _create_type_name(msg); // + "Request_"; + this->setName(name.c_str()); + + set_members(msg); +} + +ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t * members) +{ + assert(members); + + auto msg = static_cast( + members->response_members_->data); + std::string name = _create_type_name(msg); // + "Response_"; + this->setName(name.c_str()); + + set_members(msg); +} diff --git a/rmw_zenoh_cpp/src/detail/type_support.cpp b/rmw_zenoh_cpp/src/detail/type_support.cpp new file mode 100644 index 00000000..78d40aac --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/type_support.cpp @@ -0,0 +1,126 @@ +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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. + +// This file is originally from: +// https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp + +#include "rmw/error_handling.h" + +#include "type_support.hpp" + +TypeSupport::TypeSupport() +{ + m_isGetKeyDefined = false; + max_size_bound_ = false; + is_plain_ = false; +} + +void TypeSupport::set_members(const message_type_support_callbacks_t * members) +{ + members_ = members; + +#ifdef ROSIDL_TYPESUPPORT_FASTRTPS_HAS_PLAIN_TYPES + char bounds_info; + auto data_size = static_cast(members->max_serialized_size(bounds_info)); + max_size_bound_ = 0 != (bounds_info & ROSIDL_TYPESUPPORT_FASTRTPS_BOUNDED_TYPE); + is_plain_ = bounds_info == ROSIDL_TYPESUPPORT_FASTRTPS_PLAIN_TYPE; +#else + is_plain_ = true; + auto data_size = static_cast(members->max_serialized_size(is_plain_)); + max_size_bound_ = is_plain_; +#endif + + // A plain message of size 0 is an empty message + if (is_plain_ && (data_size == 0) ) { + has_data_ = false; + ++data_size; // Dummy byte + } else { + has_data_ = true; + } + + // Total size is encapsulation size + data size + m_typeSize = 4 + data_size; + // Account for RTPS submessage alignment + m_typeSize = (m_typeSize + 3) & ~3; +} + +size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) const +{ + if (is_plain_) { + return m_typeSize; + } + + assert(ros_message); + assert(impl); + + auto callbacks = static_cast(impl); + + // Encapsulation size + message size + return 4 + callbacks->get_serialized_size(ros_message); +} + +bool TypeSupport::serializeROSmessage( + const void * ros_message, + eprosima::fastcdr::Cdr & ser, + const void * impl) const +{ + assert(ros_message); + assert(impl); + + // Serialize encapsulation + ser.serialize_encapsulation(); + + // If type is not empty, serialize message + if (has_data_) { + auto callbacks = static_cast(impl); + return callbacks->cdr_serialize(ros_message, ser); + } + + // Otherwise, add a dummy byte + ser << (uint8_t)0; + return true; +} + +bool TypeSupport::deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, + void * ros_message, + const void * impl) const +{ + assert(ros_message); + assert(impl); + + try { + // Deserialize encapsulation. + deser.read_encapsulation(); + + // If type is not empty, deserialize message + if (has_data_) { + auto callbacks = static_cast(impl); + return callbacks->cdr_deserialize(deser, ros_message); + } + + // Otherwise, consume dummy byte + uint8_t dump = 0; + deser >> dump; + (void)dump; + } catch (const eprosima::fastcdr::exception::Exception &) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Fast CDR exception deserializing message of type %s.", + getName()); + return false; + } + + return true; +} diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.cpp b/rmw_zenoh_cpp/src/detail/type_support_common.cpp index 89dbfdff..0186f4e5 100644 --- a/rmw_zenoh_cpp/src/detail/type_support_common.cpp +++ b/rmw_zenoh_cpp/src/detail/type_support_common.cpp @@ -13,162 +13,34 @@ // See the License for the specific language governing permissions and // limitations under the License. -// This file is originally from: +// Parts of this file are originally from: // https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp #include +#include #include "rmw/error_handling.h" -#include "type_support_common.hpp" - -///============================================================================== -TypeSupport::TypeSupport() -{ - m_isGetKeyDefined = false; - max_size_bound_ = false; - is_plain_ = false; -} - -///============================================================================== -void TypeSupport::set_members(const message_type_support_callbacks_t * members) -{ - members_ = members; -#ifdef ROSIDL_TYPESUPPORT_FASTRTPS_HAS_PLAIN_TYPES - char bounds_info; - auto data_size = static_cast(members->max_serialized_size(bounds_info)); - max_size_bound_ = 0 != (bounds_info & ROSIDL_TYPESUPPORT_FASTRTPS_BOUNDED_TYPE); - is_plain_ = bounds_info == ROSIDL_TYPESUPPORT_FASTRTPS_PLAIN_TYPE; -#else - is_plain_ = true; - auto data_size = static_cast(members->max_serialized_size(is_plain_)); - max_size_bound_ = is_plain_; -#endif +#include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" - // A plain message of size 0 is an empty message - if (is_plain_ && (data_size == 0) ) { - has_data_ = false; - ++data_size; // Dummy byte - } else { - has_data_ = true; - } - - // Total size is encapsulation size + data size - m_typeSize = 4 + data_size; - // Account for RTPS submessage alignment - m_typeSize = (m_typeSize + 3) & ~3; -} +#include "type_support_common.hpp" -///============================================================================== -size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) const +std::string +_create_type_name( + const message_type_support_callbacks_t * members) { - if (is_plain_) { - return m_typeSize; + if (!members) { + RMW_SET_ERROR_MSG("members handle is null"); + return ""; } + std::string message_namespace(members->message_namespace_); + std::string message_name(members->message_name_); - assert(ros_message); - assert(impl); - - auto callbacks = static_cast(impl); - - // Encapsulation size + message size - return 4 + callbacks->get_serialized_size(ros_message); -} - -///============================================================================== -bool TypeSupport::serializeROSmessage( - const void * ros_message, - eprosima::fastcdr::Cdr & ser, - const void * impl) const -{ - assert(ros_message); - assert(impl); - - // Serialize encapsulation - ser.serialize_encapsulation(); - - // If type is not empty, serialize message - if (has_data_) { - auto callbacks = static_cast(impl); - return callbacks->cdr_serialize(ros_message, ser); + std::ostringstream ss; + if (!message_namespace.empty()) { + ss << message_namespace << "::"; } + ss << "dds_::" << message_name << "_"; - // Otherwise, add a dummy byte - ser << (uint8_t)0; - return true; -} - -///============================================================================== -bool TypeSupport::deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, - void * ros_message, - const void * impl) const -{ - assert(ros_message); - assert(impl); - - try { - // Deserialize encapsulation. - deser.read_encapsulation(); - - // If type is not empty, deserialize message - if (has_data_) { - auto callbacks = static_cast(impl); - return callbacks->cdr_deserialize(deser, ros_message); - } - - // Otherwise, consume dummy byte - uint8_t dump = 0; - deser >> dump; - (void)dump; - } catch (const eprosima::fastcdr::exception::Exception &) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Fast CDR exception deserializing message of type %s.", - getName()); - return false; - } - - return true; -} - -///============================================================================== -MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * members) -{ - assert(members); - - std::string name = _create_type_name(members); - this->setName(name.c_str()); - - set_members(members); -} - -///============================================================================== -ServiceTypeSupport::ServiceTypeSupport() -{ -} - -///============================================================================== -RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * members) -{ - assert(members); - - auto msg = static_cast( - members->request_members_->data); - std::string name = _create_type_name(msg); // + "Request_"; - this->setName(name.c_str()); - - set_members(msg); -} - -///============================================================================== -ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t * members) -{ - assert(members); - - auto msg = static_cast( - members->response_members_->data); - std::string name = _create_type_name(msg); // + "Response_"; - this->setName(name.c_str()); - - set_members(msg); + return ss.str(); } diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.hpp b/rmw_zenoh_cpp/src/detail/type_support_common.hpp index 712de5d5..1a419d62 100644 --- a/rmw_zenoh_cpp/src/detail/type_support_common.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support_common.hpp @@ -19,51 +19,20 @@ #ifndef DETAIL__TYPE_SUPPORT_COMMON_HPP_ #define DETAIL__TYPE_SUPPORT_COMMON_HPP_ -#include #include -#include "rmw/error_handling.h" - #include "type_support.hpp" -#include "message_type_support.hpp" -#include "service_type_support.hpp" - -#include "identifier.hpp" #include "rosidl_typesupport_fastrtps_c/identifier.h" #include "rosidl_typesupport_fastrtps_cpp/identifier.hpp" #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" -#include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" + #define RMW_FASTRTPS_CPP_TYPESUPPORT_C rosidl_typesupport_fastrtps_c__identifier #define RMW_FASTRTPS_CPP_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp::typesupport_identifier #define RMW_ZENOH_CPP_TYPESUPPORT_C rosidl_typesupport_fastrtps_c__identifier #define RMW_ZENOH_CPP_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp::typesupport_identifier -inline std::string -_create_type_name( - std::string message_namespace, - std::string message_name) -{ - std::ostringstream ss; - if (!message_namespace.empty()) { - ss << message_namespace << "::"; - } - ss << "dds_::" << message_name << "_"; - return ss.str(); -} - -inline std::string -_create_type_name( - const message_type_support_callbacks_t * members) -{ - if (!members) { - RMW_SET_ERROR_MSG("members handle is null"); - return ""; - } - std::string message_namespace(members->message_namespace_); - std::string message_name(members->message_name_); - return _create_type_name(message_namespace, message_name); -} +std::string _create_type_name(const message_type_support_callbacks_t * members); #endif // DETAIL__TYPE_SUPPORT_COMMON_HPP_ From c56df7c98423123bad02aa38525ee967d4970f62 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 5 Oct 2023 08:57:47 -0400 Subject: [PATCH 39/50] Remove dependency on rmw_dds_common. (#25) We need to compute totally different qos profiles for Zenoh anyway. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 2 -- rmw_zenoh_cpp/package.xml | 1 - rmw_zenoh_cpp/src/rmw_qos.cpp | 27 ++++++++++++++++++++++++--- 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 713dd44f..ba6acf19 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -21,7 +21,6 @@ find_package(rcutils REQUIRED) find_package(rosidl_typesupport_fastrtps_c REQUIRED) find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) find_package(rmw REQUIRED) -find_package(rmw_dds_common REQUIRED) find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(zenoh_c_vendor REQUIRED) find_package(zenohc REQUIRED) @@ -54,7 +53,6 @@ target_link_libraries(rmw_zenoh_cpp rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp rmw::rmw - rmw_dds_common::rmw_dds_common_library rmw_fastrtps_shared_cpp::rmw_fastrtps_shared_cpp zenohc::lib ) diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index e68ecfd7..414cdb64 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -17,7 +17,6 @@ rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp rmw - rmw_dds_common rmw_fastrtps_shared_cpp ament_lint_auto diff --git a/rmw_zenoh_cpp/src/rmw_qos.cpp b/rmw_zenoh_cpp/src/rmw_qos.cpp index 539adf54..8ae5b4f2 100644 --- a/rmw_zenoh_cpp/src/rmw_qos.cpp +++ b/rmw_zenoh_cpp/src/rmw_qos.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw_dds_common/qos.hpp" +#include "rmw/error_handling.h" #include "rmw/types.h" #include "rmw/qos_profiles.h" @@ -26,7 +26,28 @@ rmw_qos_profile_check_compatible( char * reason, size_t reason_size) { - return rmw_dds_common::qos_profile_check_compatible( - publisher_profile, subscription_profile, compatibility, reason, reason_size); + if (!compatibility) { + RMW_SET_ERROR_MSG("compatibility parameter is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!reason && reason_size != 0u) { + RMW_SET_ERROR_MSG("reason parameter is null, but reason_size parameter is not zero"); + return RMW_RET_INVALID_ARGUMENT; + } + + // Presume profiles are compatible until proven otherwise + *compatibility = RMW_QOS_COMPATIBILITY_OK; + + // Initialize reason buffer + if (reason && reason_size != 0u) { + reason[0] = '\0'; + } + + // TODO(clalancette): check compatibility in Zenoh QOS profiles. + (void)publisher_profile; + (void)subscription_profile; + + return RMW_RET_OK; } } // extern "C" From 7e645ab2987517147e628372d18f7d9f38875fcb Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 5 Oct 2023 09:07:50 -0400 Subject: [PATCH 40/50] Remove dependency on rmw_fastrtps_cpp. (#26) We are still taking a dependency on fastrtps for now, but hopefully we can get rid of that soon as well. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 8 +- rmw_zenoh_cpp/package.xml | 5 + rmw_zenoh_cpp/src/detail/type_support.cpp | 135 ++++++++++++++++++++++ rmw_zenoh_cpp/src/detail/type_support.hpp | 73 ++++++++++-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 1 + 5 files changed, 213 insertions(+), 9 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index ba6acf19..5323e51d 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -21,10 +21,14 @@ find_package(rcutils REQUIRED) find_package(rosidl_typesupport_fastrtps_c REQUIRED) find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) find_package(rmw REQUIRED) -find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(zenoh_c_vendor REQUIRED) find_package(zenohc REQUIRED) +find_package(fastrtps_cmake_module REQUIRED) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps 2.10 REQUIRED CONFIG) +find_package(FastRTPS 2.10 REQUIRED MODULE) + add_library(rmw_zenoh_cpp SHARED src/detail/identifier.cpp src/detail/guard_condition.cpp @@ -48,12 +52,12 @@ add_library(rmw_zenoh_cpp SHARED target_link_libraries(rmw_zenoh_cpp PRIVATE fastcdr + fastrtps rcpputils::rcpputils rcutils::rcutils rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp rmw::rmw - rmw_fastrtps_shared_cpp::rmw_fastrtps_shared_cpp zenohc::lib ) diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index 414cdb64..bb5824e5 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -10,6 +10,11 @@ ament_cmake zenoh_c_vendor + fastrtps + fastrtps_cmake_module + + fastrtps + fastrtps_cmake_module fastcdr rcpputils diff --git a/rmw_zenoh_cpp/src/detail/type_support.cpp b/rmw_zenoh_cpp/src/detail/type_support.cpp index 78d40aac..b62e7969 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/type_support.cpp @@ -16,6 +16,13 @@ // This file is originally from: // https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp +#include +#include +#include + +#include "fastrtps/types/DynamicData.h" +#include "fastrtps/types/DynamicPubSubType.h" + #include "rmw/error_handling.h" #include "type_support.hpp" @@ -25,6 +32,8 @@ TypeSupport::TypeSupport() m_isGetKeyDefined = false; max_size_bound_ = false; is_plain_ = false; + auto_fill_type_object(false); + auto_fill_type_information(false); } void TypeSupport::set_members(const message_type_support_callbacks_t * members) @@ -124,3 +133,129 @@ bool TypeSupport::deserializeROSmessage( return true; } + +void TypeSupport::deleteData(void * data) +{ + assert(data); + delete static_cast(data); +} + +void * TypeSupport::createData() +{ + return new eprosima::fastcdr::FastBuffer(); +} + +bool TypeSupport::serialize( + void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) +{ + assert(data); + assert(payload); + + auto ser_data = static_cast(data); + + switch (ser_data->type) { + case FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: + { + eprosima::fastcdr::FastBuffer fastbuffer( // Object that manages the raw buffer + reinterpret_cast(payload->data), payload->max_size); + eprosima::fastcdr::Cdr ser( // Object that serializes the data + fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + if (this->serializeROSmessage(ser_data->data, ser, ser_data->impl)) { + payload->encapsulation = ser.endianness() == + eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + payload->length = (uint32_t)ser.getSerializedDataLength(); + return true; + } + break; + } + + case FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER: + { + auto ser = static_cast(ser_data->data); + if (payload->max_size >= ser->getSerializedDataLength()) { + payload->length = static_cast(ser->getSerializedDataLength()); + payload->encapsulation = ser->endianness() == + eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + memcpy(payload->data, ser->getBufferPointer(), ser->getSerializedDataLength()); + return true; + } + break; + } + + case FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: + { + auto m_type = std::make_shared(); + + // Serializes payload into dynamic data stored in data->data + return m_type->serialize( + static_cast(ser_data->data), payload + ); + } + + default: + return false; + } + return false; +} + +bool TypeSupport::deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t * payload, + void * data) +{ + assert(data); + assert(payload); + + auto ser_data = static_cast(data); + + switch (ser_data->type) { + case FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: + { + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(payload->data), payload->length); + eprosima::fastcdr::Cdr deser( + fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + return deserializeROSmessage(deser, ser_data->data, ser_data->impl); + } + + case FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER: + { + auto buffer = static_cast(ser_data->data); + if (!buffer->reserve(payload->length)) { + return false; + } + memcpy(buffer->getBuffer(), payload->data, payload->length); + return true; + } + + case FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: + { + auto m_type = std::make_shared(); + + // Deserializes payload into dynamic data stored in data->data (copies!) + return m_type->deserialize( + payload, static_cast(ser_data->data) + ); + } + + default: + return false; + } + return false; +} + +std::function TypeSupport::getSerializedSizeProvider(void * data) +{ + assert(data); + + auto ser_data = static_cast(data); + auto ser_size = [this, ser_data]() -> uint32_t + { + if (ser_data->type == FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER) { + auto ser = static_cast(ser_data->data); + return static_cast(ser->getSerializedDataLength()); + } + return static_cast( + this->getEstimatedSerializedSize(ser_data->data, ser_data->impl)); + }; + return ser_size; +} diff --git a/rmw_zenoh_cpp/src/detail/type_support.hpp b/rmw_zenoh_cpp/src/detail/type_support.hpp index caa15ecf..9c75a415 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support.hpp @@ -19,26 +19,85 @@ #ifndef DETAIL__TYPE_SUPPORT_HPP_ #define DETAIL__TYPE_SUPPORT_HPP_ -#include +#include + +#include "fastcdr/Cdr.h" + +#include "fastdds/dds/topic/TopicDataType.hpp" #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" -#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +enum SerializedDataType +{ + FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER, + FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE, + FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE +}; + +// Publishers write method will receive a pointer to this struct +struct SerializedData +{ + SerializedDataType type; // The type of the next field + void * data; + const void * impl; // RMW implementation specific data +}; -class TypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport +class TypeSupport : public eprosima::fastdds::dds::TopicDataType { public: - size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const override; + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const; bool serializeROSmessage( - const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const override; + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const; bool deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const; - TypeSupport(); + bool getKey( + void * data, + eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, + bool force_md5 = false) override + { + (void)data; + (void)ihandle; + (void)force_md5; + return false; + } + + bool serialize(void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) override; + + bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t * payload, void * data) override; + + std::function getSerializedSizeProvider(void * data) override; + + void * createData() override; + + void deleteData(void * data) override; + + inline bool is_bounded() const +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + override +#endif + { + return max_size_bound_; + } + + inline bool is_plain() const +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + override +#endif + { + return is_plain_; + } + + virtual ~TypeSupport() {} protected: + TypeSupport(); + + bool max_size_bound_; + bool is_plain_; + void set_members(const message_type_support_callbacks_t * members); private: diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 87afb79b..aa682b56 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -29,6 +29,7 @@ #include "rcpputils/scope_exit.hpp" #include "rcutils/env.h" +#include "rcutils/logging_macros.h" #include "rcutils/strdup.h" #include "rcutils/types.h" From 2e635f5befa5ab80f932e5bdd632bdd9b572243e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 5 Oct 2023 09:18:13 -0400 Subject: [PATCH 41/50] Completely remove dependency on Fast-DDS (#27) We do this by defining the TypeSupport class ourself. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 7 - rmw_zenoh_cpp/package.xml | 6 - .../src/detail/message_type_support.cpp | 1 + rmw_zenoh_cpp/src/detail/type_support.cpp | 149 ++---------------- rmw_zenoh_cpp/src/detail/type_support.hpp | 50 ++---- .../src/detail/type_support_common.hpp | 5 - rmw_zenoh_cpp/src/rmw_zenoh.cpp | 4 +- 7 files changed, 27 insertions(+), 195 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 5323e51d..560fd0a0 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -15,7 +15,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(fastcdr CONFIG REQUIRED) -find_package(fastrtps_cmake_module REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rosidl_typesupport_fastrtps_c REQUIRED) @@ -24,11 +23,6 @@ find_package(rmw REQUIRED) find_package(zenoh_c_vendor REQUIRED) find_package(zenohc REQUIRED) -find_package(fastrtps_cmake_module REQUIRED) -find_package(fastcdr REQUIRED CONFIG) -find_package(fastrtps 2.10 REQUIRED CONFIG) -find_package(FastRTPS 2.10 REQUIRED MODULE) - add_library(rmw_zenoh_cpp SHARED src/detail/identifier.cpp src/detail/guard_condition.cpp @@ -52,7 +46,6 @@ add_library(rmw_zenoh_cpp SHARED target_link_libraries(rmw_zenoh_cpp PRIVATE fastcdr - fastrtps rcpputils::rcpputils rcutils::rcutils rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index bb5824e5..7d27fe9c 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -10,11 +10,6 @@ ament_cmake zenoh_c_vendor - fastrtps - fastrtps_cmake_module - - fastrtps - fastrtps_cmake_module fastcdr rcpputils @@ -22,7 +17,6 @@ rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp rmw - rmw_fastrtps_shared_cpp ament_lint_auto ament_lint_common diff --git a/rmw_zenoh_cpp/src/detail/message_type_support.cpp b/rmw_zenoh_cpp/src/detail/message_type_support.cpp index 861d0db7..208dbe2f 100644 --- a/rmw_zenoh_cpp/src/detail/message_type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/message_type_support.cpp @@ -16,6 +16,7 @@ // Parts of this file are originally from: // https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp +#include #include #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" diff --git a/rmw_zenoh_cpp/src/detail/type_support.cpp b/rmw_zenoh_cpp/src/detail/type_support.cpp index b62e7969..82c7a7cb 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/type_support.cpp @@ -20,20 +20,25 @@ #include #include -#include "fastrtps/types/DynamicData.h" -#include "fastrtps/types/DynamicPubSubType.h" - #include "rmw/error_handling.h" #include "type_support.hpp" TypeSupport::TypeSupport() { - m_isGetKeyDefined = false; max_size_bound_ = false; is_plain_ = false; - auto_fill_type_object(false); - auto_fill_type_information(false); + type_size_ = 0; +} + +void TypeSupport::setName(const char * name) +{ + topic_data_type_name_ = std::string(name); +} + +const char * TypeSupport::getName() const +{ + return topic_data_type_name_.c_str(); } void TypeSupport::set_members(const message_type_support_callbacks_t * members) @@ -60,15 +65,15 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members) } // Total size is encapsulation size + data size - m_typeSize = 4 + data_size; + type_size_ = 4 + data_size; // Account for RTPS submessage alignment - m_typeSize = (m_typeSize + 3) & ~3; + type_size_ = (type_size_ + 3) & ~3; } size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) const { if (is_plain_) { - return m_typeSize; + return type_size_; } assert(ros_message); @@ -133,129 +138,3 @@ bool TypeSupport::deserializeROSmessage( return true; } - -void TypeSupport::deleteData(void * data) -{ - assert(data); - delete static_cast(data); -} - -void * TypeSupport::createData() -{ - return new eprosima::fastcdr::FastBuffer(); -} - -bool TypeSupport::serialize( - void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) -{ - assert(data); - assert(payload); - - auto ser_data = static_cast(data); - - switch (ser_data->type) { - case FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: - { - eprosima::fastcdr::FastBuffer fastbuffer( // Object that manages the raw buffer - reinterpret_cast(payload->data), payload->max_size); - eprosima::fastcdr::Cdr ser( // Object that serializes the data - fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - if (this->serializeROSmessage(ser_data->data, ser, ser_data->impl)) { - payload->encapsulation = ser.endianness() == - eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; - payload->length = (uint32_t)ser.getSerializedDataLength(); - return true; - } - break; - } - - case FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER: - { - auto ser = static_cast(ser_data->data); - if (payload->max_size >= ser->getSerializedDataLength()) { - payload->length = static_cast(ser->getSerializedDataLength()); - payload->encapsulation = ser->endianness() == - eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; - memcpy(payload->data, ser->getBufferPointer(), ser->getSerializedDataLength()); - return true; - } - break; - } - - case FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: - { - auto m_type = std::make_shared(); - - // Serializes payload into dynamic data stored in data->data - return m_type->serialize( - static_cast(ser_data->data), payload - ); - } - - default: - return false; - } - return false; -} - -bool TypeSupport::deserialize( - eprosima::fastrtps::rtps::SerializedPayload_t * payload, - void * data) -{ - assert(data); - assert(payload); - - auto ser_data = static_cast(data); - - switch (ser_data->type) { - case FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE: - { - eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(payload->data), payload->length); - eprosima::fastcdr::Cdr deser( - fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - return deserializeROSmessage(deser, ser_data->data, ser_data->impl); - } - - case FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER: - { - auto buffer = static_cast(ser_data->data); - if (!buffer->reserve(payload->length)) { - return false; - } - memcpy(buffer->getBuffer(), payload->data, payload->length); - return true; - } - - case FASTRTPS_SERIALIZED_DATA_TYPE_DYNAMIC_MESSAGE: - { - auto m_type = std::make_shared(); - - // Deserializes payload into dynamic data stored in data->data (copies!) - return m_type->deserialize( - payload, static_cast(ser_data->data) - ); - } - - default: - return false; - } - return false; -} - -std::function TypeSupport::getSerializedSizeProvider(void * data) -{ - assert(data); - - auto ser_data = static_cast(data); - auto ser_size = [this, ser_data]() -> uint32_t - { - if (ser_data->type == FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER) { - auto ser = static_cast(ser_data->data); - return static_cast(ser->getSerializedDataLength()); - } - return static_cast( - this->getEstimatedSerializedSize(ser_data->data, ser_data->impl)); - }; - return ser_size; -} diff --git a/rmw_zenoh_cpp/src/detail/type_support.hpp b/rmw_zenoh_cpp/src/detail/type_support.hpp index 9c75a415..8590b95b 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support.hpp @@ -20,11 +20,10 @@ #define DETAIL__TYPE_SUPPORT_HPP_ #include +#include #include "fastcdr/Cdr.h" -#include "fastdds/dds/topic/TopicDataType.hpp" - #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" enum SerializedDataType @@ -42,9 +41,13 @@ struct SerializedData const void * impl; // RMW implementation specific data }; -class TypeSupport : public eprosima::fastdds::dds::TopicDataType +class TypeSupport { public: + void setName(const char * name); + + const char * getName() const; + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const; bool serializeROSmessage( @@ -53,43 +56,6 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType bool deserializeROSmessage( eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const; - bool getKey( - void * data, - eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, - bool force_md5 = false) override - { - (void)data; - (void)ihandle; - (void)force_md5; - return false; - } - - bool serialize(void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) override; - - bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t * payload, void * data) override; - - std::function getSerializedSizeProvider(void * data) override; - - void * createData() override; - - void deleteData(void * data) override; - - inline bool is_bounded() const -#ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED - override -#endif - { - return max_size_bound_; - } - - inline bool is_plain() const -#ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN - override -#endif - { - return is_plain_; - } - virtual ~TypeSupport() {} protected: @@ -103,6 +69,10 @@ class TypeSupport : public eprosima::fastdds::dds::TopicDataType private: const message_type_support_callbacks_t * members_; bool has_data_; + + uint32_t type_size_; + + std::string topic_data_type_name_; }; #endif // DETAIL__TYPE_SUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.hpp b/rmw_zenoh_cpp/src/detail/type_support_common.hpp index 1a419d62..b80f732f 100644 --- a/rmw_zenoh_cpp/src/detail/type_support_common.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support_common.hpp @@ -21,15 +21,10 @@ #include -#include "type_support.hpp" - #include "rosidl_typesupport_fastrtps_c/identifier.h" #include "rosidl_typesupport_fastrtps_cpp/identifier.hpp" #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" -#define RMW_FASTRTPS_CPP_TYPESUPPORT_C rosidl_typesupport_fastrtps_c__identifier -#define RMW_FASTRTPS_CPP_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp::typesupport_identifier - #define RMW_ZENOH_CPP_TYPESUPPORT_C rosidl_typesupport_fastrtps_c__identifier #define RMW_ZENOH_CPP_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp::typesupport_identifier diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index aa682b56..eb302d10 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -280,11 +280,11 @@ static const rosidl_message_type_support_t * find_type_support( const rosidl_message_type_support_t * type_supports) { const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); + type_supports, RMW_ZENOH_CPP_TYPESUPPORT_C); if (!type_support) { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); - type_support = get_message_typesupport_handle(type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); + type_support = get_message_typesupport_handle(type_supports, RMW_ZENOH_CPP_TYPESUPPORT_CPP); if (!type_support) { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); From a4aa37039096e3e2986d174c3792c76c8e482539 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 5 Oct 2023 09:29:27 -0400 Subject: [PATCH 42/50] Rename typesupport methods. (#28) So they conform with the ROS 2 standards. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/src/detail/message_type_support.cpp | 2 +- rmw_zenoh_cpp/src/detail/message_type_support.hpp | 2 +- rmw_zenoh_cpp/src/detail/service_type_support.cpp | 4 ++-- rmw_zenoh_cpp/src/detail/service_type_support.hpp | 4 ++-- rmw_zenoh_cpp/src/detail/type_support.cpp | 12 ++++++------ rmw_zenoh_cpp/src/detail/type_support.hpp | 10 +++++----- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 12 ++++++------ 7 files changed, 23 insertions(+), 23 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/message_type_support.cpp b/rmw_zenoh_cpp/src/detail/message_type_support.cpp index 208dbe2f..e270a12a 100644 --- a/rmw_zenoh_cpp/src/detail/message_type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/message_type_support.cpp @@ -29,7 +29,7 @@ MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * assert(members); std::string name = _create_type_name(members); - this->setName(name.c_str()); + this->set_name(name.c_str()); set_members(members); } diff --git a/rmw_zenoh_cpp/src/detail/message_type_support.hpp b/rmw_zenoh_cpp/src/detail/message_type_support.hpp index 87a16780..538064f5 100644 --- a/rmw_zenoh_cpp/src/detail/message_type_support.hpp +++ b/rmw_zenoh_cpp/src/detail/message_type_support.hpp @@ -23,7 +23,7 @@ #include "type_support.hpp" ///============================================================================== -class MessageTypeSupport : public TypeSupport +class MessageTypeSupport final : public TypeSupport { public: explicit MessageTypeSupport(const message_type_support_callbacks_t * members); diff --git a/rmw_zenoh_cpp/src/detail/service_type_support.cpp b/rmw_zenoh_cpp/src/detail/service_type_support.cpp index 6b471715..4cdee389 100644 --- a/rmw_zenoh_cpp/src/detail/service_type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/service_type_support.cpp @@ -34,7 +34,7 @@ RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * auto msg = static_cast( members->request_members_->data); std::string name = _create_type_name(msg); // + "Request_"; - this->setName(name.c_str()); + this->set_name(name.c_str()); set_members(msg); } @@ -46,7 +46,7 @@ ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t auto msg = static_cast( members->response_members_->data); std::string name = _create_type_name(msg); // + "Response_"; - this->setName(name.c_str()); + this->set_name(name.c_str()); set_members(msg); } diff --git a/rmw_zenoh_cpp/src/detail/service_type_support.hpp b/rmw_zenoh_cpp/src/detail/service_type_support.hpp index 48a8776d..0e87bfca 100644 --- a/rmw_zenoh_cpp/src/detail/service_type_support.hpp +++ b/rmw_zenoh_cpp/src/detail/service_type_support.hpp @@ -29,13 +29,13 @@ class ServiceTypeSupport : public TypeSupport ServiceTypeSupport(); }; -class RequestTypeSupport : public ServiceTypeSupport +class RequestTypeSupport final : public ServiceTypeSupport { public: explicit RequestTypeSupport(const service_type_support_callbacks_t * members); }; -class ResponseTypeSupport : public ServiceTypeSupport +class ResponseTypeSupport final : public ServiceTypeSupport { public: explicit ResponseTypeSupport(const service_type_support_callbacks_t * members); diff --git a/rmw_zenoh_cpp/src/detail/type_support.cpp b/rmw_zenoh_cpp/src/detail/type_support.cpp index 82c7a7cb..8816694a 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/type_support.cpp @@ -31,12 +31,12 @@ TypeSupport::TypeSupport() type_size_ = 0; } -void TypeSupport::setName(const char * name) +void TypeSupport::set_name(const char * name) { topic_data_type_name_ = std::string(name); } -const char * TypeSupport::getName() const +const char * TypeSupport::get_name() const { return topic_data_type_name_.c_str(); } @@ -70,7 +70,7 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members) type_size_ = (type_size_ + 3) & ~3; } -size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) const +size_t TypeSupport::get_estimated_serialized_size(const void * ros_message, const void * impl) const { if (is_plain_) { return type_size_; @@ -85,7 +85,7 @@ size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const v return 4 + callbacks->get_serialized_size(ros_message); } -bool TypeSupport::serializeROSmessage( +bool TypeSupport::serialize_ros_message( const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const @@ -107,7 +107,7 @@ bool TypeSupport::serializeROSmessage( return true; } -bool TypeSupport::deserializeROSmessage( +bool TypeSupport::deserialize_ros_message( eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const @@ -132,7 +132,7 @@ bool TypeSupport::deserializeROSmessage( } catch (const eprosima::fastcdr::exception::Exception &) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "Fast CDR exception deserializing message of type %s.", - getName()); + get_name()); return false; } diff --git a/rmw_zenoh_cpp/src/detail/type_support.hpp b/rmw_zenoh_cpp/src/detail/type_support.hpp index 8590b95b..d90dcdcf 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support.hpp @@ -44,16 +44,16 @@ struct SerializedData class TypeSupport { public: - void setName(const char * name); + void set_name(const char * name); - const char * getName() const; + const char * get_name() const; - size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const; + size_t get_estimated_serialized_size(const void * ros_message, const void * impl) const; - bool serializeROSmessage( + bool serialize_ros_message( const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const; - bool deserializeROSmessage( + bool deserialize_ros_message( eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const; virtual ~TypeSupport() {} diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index eb302d10..3ec29be5 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -617,7 +617,7 @@ rmw_publish( rcutils_allocator_t * allocator = &(publisher_data->context->options.allocator); // Serialize data. - size_t max_data_length = publisher_data->type_support->getEstimatedSerializedSize( + size_t max_data_length = publisher_data->type_support->get_estimated_serialized_size( ros_message, publisher_data->type_support_impl); @@ -638,7 +638,7 @@ rmw_publish( fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - if (!publisher_data->type_support->serializeROSmessage( + if (!publisher_data->type_support->serialize_ros_message( ros_message, ser, publisher_data->type_support_impl)) @@ -816,7 +816,7 @@ rmw_serialize( auto callbacks = static_cast(ts->data); auto tss = MessageTypeSupport(callbacks); - auto data_length = tss.getEstimatedSerializedSize(ros_message, callbacks); + auto data_length = tss.get_estimated_serialized_size(ros_message, callbacks); if (serialized_message->buffer_capacity < data_length) { if (rmw_serialized_message_resize(serialized_message, data_length) != RMW_RET_OK) { RMW_SET_ERROR_MSG("unable to dynamically resize serialized message"); @@ -829,7 +829,7 @@ rmw_serialize( eprosima::fastcdr::Cdr ser( buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - auto ret = tss.serializeROSmessage(ros_message, ser, callbacks); + auto ret = tss.serialize_ros_message(ros_message, ser, callbacks); serialized_message->buffer_length = data_length; serialized_message->buffer_capacity = data_length; return ret == true ? RMW_RET_OK : RMW_RET_ERROR; @@ -856,7 +856,7 @@ rmw_deserialize( eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - auto ret = tss.deserializeROSmessage(deser, ros_message, callbacks); + auto ret = tss.deserialize_ros_message(deser, ros_message, callbacks); return ret == true ? RMW_RET_OK : RMW_RET_ERROR; } @@ -1236,7 +1236,7 @@ static rmw_ret_t __rmw_take( fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); - if (!sub_data->type_support->deserializeROSmessage( + if (!sub_data->type_support->deserialize_ros_message( deser, ros_message, sub_data->type_support_impl)) From 8b1a304c8c9501d40e38f801f003beca62806d23 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 5 Oct 2023 21:37:53 -0400 Subject: [PATCH 43/50] Take ownership of subscription data, rather than taking a copy. (#29) This allows us to do less allocations and less copies during the reception path. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 20 +++----------------- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 7 +++---- rmw_zenoh_cpp/src/rmw_init.cpp | 3 --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 7 +++---- 4 files changed, 9 insertions(+), 28 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index ea421542..0458bfd0 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -39,21 +39,6 @@ void sub_data_handler( return; } - rcutils_allocator_t * allocator = &sub_data->context->options.allocator; - - uint8_t * cdr_buffer = - static_cast(allocator->allocate(sample->payload.len, allocator->state)); - if (cdr_buffer == nullptr) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Failed to allocate memory for cdr_buffer for " - "subscription for %s", - z_loan(keystr) - ); - return; - } - memcpy(cdr_buffer, sample->payload.start, sample->payload.len); - { std::lock_guard lock(sub_data->message_queue_mutex); @@ -67,13 +52,14 @@ void sub_data_handler( z_loan(keystr)); std::unique_ptr old = std::move(sub_data->message_queue.back()); - allocator->deallocate(old->data, allocator->state); + z_drop(&old->payload); sub_data->message_queue.pop_back(); } sub_data->message_queue.emplace_front( std::make_unique( - sample->payload.len, cdr_buffer, sample->timestamp.time, sample->timestamp.id.id)); + zc_sample_payload_rcinc(sample), + sample->timestamp.time, sample->timestamp.id.id)); // Since we added new data, trigger the guard condition if it is available std::lock_guard internal_lock(sub_data->internal_mutex); diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 3346ab8f..e4295f2a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -87,14 +87,13 @@ void sub_data_handler(const z_sample_t * sample, void * sub_data); struct saved_msg_data { - explicit saved_msg_data(size_t len, uint8_t * d, uint64_t recv_ts, const uint8_t pub_gid[16]) - : data_length(len), data(d), recv_timestamp(recv_ts) + explicit saved_msg_data(zc_owned_payload_t p, uint64_t recv_ts, const uint8_t pub_gid[16]) + : payload(p), recv_timestamp(recv_ts) { memcpy(publisher_gid, pub_gid, 16); } - size_t data_length; - uint8_t * data; + zc_owned_payload_t payload; uint64_t recv_timestamp; uint8_t publisher_gid[16]; }; diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index c5a891a5..77bea444 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -37,7 +37,6 @@ extern "C" rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) { - fprintf(stderr, "Initializing context\n"); RMW_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_FOR_NULL_WITH_MSG( @@ -187,7 +186,6 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) rmw_ret_t rmw_shutdown(rmw_context_t * context) { - fprintf(stderr, "Shutting down context\n"); RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_FOR_NULL_WITH_MSG( context->impl, @@ -213,7 +211,6 @@ rmw_shutdown(rmw_context_t * context) rmw_ret_t rmw_context_fini(rmw_context_t * context) { - fprintf(stderr, "Finalizing context\n"); RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_FOR_NULL_WITH_MSG( context->impl, diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 3ec29be5..963cd577 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1228,8 +1228,8 @@ static rmw_ret_t __rmw_take( // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(msg_data->data), - msg_data->data_length); + reinterpret_cast(const_cast(msg_data->payload.payload.start)), + msg_data->payload.payload.len); // Object that serializes the data eprosima::fastcdr::Cdr deser( @@ -1245,9 +1245,8 @@ static rmw_ret_t __rmw_take( return RMW_RET_ERROR; } - rcutils_allocator_t * allocator = &sub_data->context->options.allocator; *taken = true; - allocator->deallocate(msg_data->data, allocator->state); + z_drop(&msg_data->payload); // TODO(clalancette): fill in source_timestamp message_info->source_timestamp = 0; From 220ec41562cc4df629a733bfc65d9fb870dd2ae6 Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 8 Nov 2023 03:49:59 +0800 Subject: [PATCH 44/50] Support shared memory publishers (#20) * Add shm manager * Get buffer from shm if available * Drop shm_manager when finalizing context * Disable shm in config which may be used as default Signed-off-by: Yadunund Co-authored-by: Chris Lalancette --- README.md | 3 ++ rmw_zenoh_config.json5 | 18 +++++++ rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 3 ++ rmw_zenoh_cpp/src/rmw_init.cpp | 48 +++++++++++++++-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 58 ++++++++++++++++----- 5 files changed, 115 insertions(+), 15 deletions(-) create mode 100644 rmw_zenoh_config.json5 diff --git a/README.md b/README.md index 431ef9a2..102401d2 100644 --- a/README.md +++ b/README.md @@ -36,6 +36,9 @@ export RMW_IMPLEMENTATION=rmw_zenoh_cpp ros2 topic pub "/chatter" std_msgs/msg/String '{data: hello}' ``` +## Config +The zenoh session may be configured by setting the `ZENOH_CONFIG_PATH` environment variable to point to the custom zenoh configuration file. The [rmw_zenoh_config.json5](./rmw_zenoh_config.json5) may be used to configure the zenoh session for this middleware. For a complete list of configurable settings, see [documentation](https://github.com/eclipse-zenoh/zenoh/blob/master/DEFAULT_CONFIG.json5). + ## TODO Features - [x] Publisher - [ ] Subscription diff --git a/rmw_zenoh_config.json5 b/rmw_zenoh_config.json5 new file mode 100644 index 00000000..9b8d8887 --- /dev/null +++ b/rmw_zenoh_config.json5 @@ -0,0 +1,18 @@ +{ + /// The node's mode (router, peer or client) + mode: "peer", + + /// The default timeout to apply to queries in milliseconds. + queries_default_timeout: 10000, + + /// Configure internal transport parameters + transport: { + qos: { + enabled: true, + }, + // Shared memory configuration + shared_memory: { + enabled: false, + }, + }, +} diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index e4295f2a..b2760ce9 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -39,6 +39,9 @@ struct rmw_context_impl_s // An owned session. z_owned_session_t session; + // The SHM manager. + zc_owned_shm_manager_t shm_manager; + /// Shutdown flag. bool is_shutdown; diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 77bea444..ace7da92 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -32,6 +32,11 @@ extern "C" { + +// Megabytes of SHM to reserve. +// TODO(clalancette): Make this configurable, or get it from the configuration +#define SHM_BUFFER_SIZE_MB 10 + //============================================================================== /// Initialize the middleware with the given options, and yielding an context. rmw_ret_t @@ -121,6 +126,13 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) } } + // Check if shm is enabled. + z_owned_str_t shm_enabled = zc_config_get(z_loan(config), "transport/shared_memory/enabled"); + auto free_shm_enabled = rcpputils::make_scope_exit( + [&shm_enabled]() { + z_drop(z_move(shm_enabled)); + }); + // Initialize the zenoh session. context->impl->session = z_open(z_move(config)); if (!z_session_check(&context->impl->session)) { @@ -132,6 +144,33 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) z_close(z_move(context->impl->session)); }); + // Initialize the shm manager if shared_memory is enabled in the config. + if (shm_enabled._cstr != nullptr && + strcmp(shm_enabled._cstr, "true") == 0) + { + z_id_t id = z_info_zid(z_loan(context->impl->session)); + char idstr[sizeof(id.id) * 2 + 1]; // 2 bytes for each byte of the id, plus the trailing \0 + for (size_t i = 0; i < sizeof(id.id); ++i) { + sprintf(idstr + 2 * i, "%02x", id.id[i]); + } + idstr[32] = 0; + // TODO(yadunund): Can we get the size of the shm from the config even though it's not + // a standard parameter? + context->impl->shm_manager = + zc_shm_manager_new( + z_loan(context->impl->session), + idstr, + SHM_BUFFER_SIZE_MB * 1024 * 1024); + if (!zc_shm_manager_check(&context->impl->shm_manager)) { + RMW_SET_ERROR_MSG("Unable to create shm manager."); + return RMW_RET_ERROR; + } + } + auto free_shm_manager = rcpputils::make_scope_exit( + [context]() { + z_drop(z_move(context->impl->shm_manager)); + }); + // Initialize the guard condition. context->impl->graph_guard_condition = static_cast(allocator->zero_allocate( @@ -169,13 +208,14 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(gc_data->~GuardCondition(), GuardCondition); }); + close_session.cancel(); destruct_guard_condition_data.cancel(); + impl_destructor.cancel(); free_guard_condition_data.cancel(); free_guard_condition.cancel(); - close_session.cancel(); - free_options.cancel(); - impl_destructor.cancel(); free_impl.cancel(); + free_options.cancel(); + free_shm_manager.cancel(); restore_context.cancel(); return RMW_RET_OK; @@ -228,6 +268,8 @@ rmw_context_fini(rmw_context_t * context) const rcutils_allocator_t * allocator = &context->options.allocator; + z_drop(z_move(context->impl->shm_manager)); + RMW_TRY_DESTRUCTOR( static_cast(context->impl->graph_guard_condition->data)->~GuardCondition(), GuardCondition, ); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 963cd577..1c3b0af4 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -621,15 +621,41 @@ rmw_publish( ros_message, publisher_data->type_support_impl); - // Init serialized message byte array - char * msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); - RMW_CHECK_FOR_NULL_WITH_MSG( - msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); + // To store serialized message byte array. + char * msg_bytes = nullptr; + bool from_shm = false; auto free_msg_bytes = rcpputils::make_scope_exit( - [msg_bytes, allocator]() { - allocator->deallocate(msg_bytes, allocator->state); + [msg_bytes, allocator, from_shm]() { + if (msg_bytes && !from_shm) { + allocator->deallocate(msg_bytes, allocator->state); + } }); + zc_owned_shmbuf_t shmbuf; + // Get memory from SHM buffer if available. + if (zc_shm_manager_check(&publisher_data->context->impl->shm_manager)) { + shmbuf = zc_shm_alloc( + &publisher_data->context->impl->shm_manager, + max_data_length); + if (!z_check(shmbuf)) { + zc_shm_gc(&publisher_data->context->impl->shm_manager); + shmbuf = zc_shm_alloc(&publisher_data->context->impl->shm_manager, max_data_length); + if (!z_check(shmbuf)) { + // TODO(Yadunund): Should we revert to regular allocation and not return an error? + RMW_SET_ERROR_MSG("Failed to allocate a SHM buffer, even after GCing"); + return RMW_RET_ERROR; + } + } + msg_bytes = (char *)zc_shmbuf_ptr(&shmbuf); + from_shm = true; + } + // Get memory from the allocator. + else { + msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); + } + // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(msg_bytes, max_data_length); @@ -649,18 +675,25 @@ rmw_publish( const size_t data_length = ser.getSerializedDataLength(); + int ret; // The encoding is simply forwarded and is useful when key expressions in the // session use different encoding formats. In our case, all key expressions // will be encoded with CDR so it does not really matter. z_publisher_put_options_t options = z_publisher_put_options_default(); options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); - // Returns 0 if success. - int8_t ret = z_publisher_put( - z_loan(publisher_data->pub), - (const uint8_t *)msg_bytes, - data_length, - &options); + if (from_shm) { + zc_shmbuf_set_length(&shmbuf, data_length); + zc_owned_payload_t payload = zc_shmbuf_into_payload(z_move(shmbuf)); + ret = zc_publisher_put_owned(z_loan(publisher_data->pub), z_move(payload), &options); + } else { + // Returns 0 if success. + ret = z_publisher_put( + z_loan(publisher_data->pub), + (const uint8_t *)msg_bytes, + data_length, + &options); + } if (ret) { RMW_SET_ERROR_MSG("unable to publish message"); return RMW_RET_ERROR; @@ -1209,6 +1242,7 @@ static rmw_ret_t __rmw_take( *taken = false; auto sub_data = static_cast(subscription->data); + RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); // RETRIEVE SERIALIZED MESSAGE =============================================== From d03ab5665585589f4a2aac20fddbe06f466d76a3 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 29 Sep 2023 14:10:28 +0000 Subject: [PATCH 45/50] Start adding in graph cache support. For now we are only tracking local publishers and subscriptons. Signed-off-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 1 + rmw_zenoh_cpp/src/detail/graph_cache.cpp | 121 +++++++++++++++ rmw_zenoh_cpp/src/detail/graph_cache.hpp | 85 +++++++++++ rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 11 ++ rmw_zenoh_cpp/src/rmw_init.cpp | 59 +++++++- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 156 ++++++++++++++++---- 6 files changed, 398 insertions(+), 35 deletions(-) create mode 100644 rmw_zenoh_cpp/src/detail/graph_cache.cpp create mode 100644 rmw_zenoh_cpp/src/detail/graph_cache.hpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 560fd0a0..bce95ece 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(zenohc REQUIRED) add_library(rmw_zenoh_cpp SHARED src/detail/identifier.cpp + src/detail/graph_cache.cpp src/detail/guard_condition.cpp src/detail/message_type_support.cpp src/detail/rmw_data_types.cpp diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp new file mode 100644 index 00000000..18280c50 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 "rcutils/strdup.h" + +#include "graph_cache.hpp" + +PublisherData::PublisherData( + const char * topic, const char * node, const char * namespace_, + const char * type, rcutils_allocator_t * allocator) +: allocator_(allocator) +{ + // TODO(clalancette): Check for error + topic_name_ = rcutils_strdup(topic, *allocator); + + // TODO(clalancette): Check for error + node_name_ = rcutils_strdup(node, *allocator); + + // TODO(clalancette): Check for error + namespace_name_ = rcutils_strdup(namespace_, *allocator); + + // TODO(clalancette): Check for error + type_name_ = rcutils_strdup(type, *allocator); +} + +PublisherData::~PublisherData() +{ + allocator_->deallocate(topic_name_, allocator_->state); + allocator_->deallocate(node_name_, allocator_->state); + allocator_->deallocate(namespace_name_, allocator_->state); + allocator_->deallocate(type_name_, allocator_->state); +} + +SubscriptionData::SubscriptionData( + const char * topic, const char * node, const char * namespace_, + const char * type, rcutils_allocator_t * allocator) +: allocator_(allocator) +{ + // TODO(clalancette): Check for error + topic_name_ = rcutils_strdup(topic, *allocator); + + // TODO(clalancette): Check for error + node_name_ = rcutils_strdup(node, *allocator); + + // TODO(clalancette): Check for error + namespace_name_ = rcutils_strdup(namespace_, *allocator); + + // TODO(clalancette): Check for error + type_name_ = rcutils_strdup(type, *allocator); +} + +SubscriptionData::~SubscriptionData() +{ + allocator_->deallocate(topic_name_, allocator_->state); + allocator_->deallocate(node_name_, allocator_->state); + allocator_->deallocate(namespace_name_, allocator_->state); + allocator_->deallocate(type_name_, allocator_->state); +} + +uint64_t +GraphCache::add_publisher( + const char * topic, const char * node, const char * namespace_, + const char * type, rcutils_allocator_t * allocator) +{ + std::lock_guard lck(publishers_mutex_); + uint64_t this_handle_id = publishers_handle_id_++; + publishers_.emplace( + std::make_pair( + this_handle_id, std::make_unique(topic, node, namespace_, type, allocator))); + return this_handle_id; +} + +void +GraphCache::remove_publisher(uint64_t handle) +{ + std::lock_guard lck(publishers_mutex_); + if (publishers_.count(handle) == 0) { + return; + } + + publishers_.erase(handle); +} + +uint64_t +GraphCache::add_subscription( + const char * topic, const char * node, const char * namespace_, + const char * type, rcutils_allocator_t * allocator) +{ + std::lock_guard lck(subscriptions_mutex_); + uint64_t this_handle_id = subscriptions_handle_id_++; + subscriptions_.emplace( + std::make_pair( + this_handle_id, + std::make_unique(topic, node, namespace_, type, allocator))); + return this_handle_id; +} + +void +GraphCache::remove_subscription(uint64_t handle) +{ + std::lock_guard lck(subscriptions_mutex_); + if (subscriptions_.count(handle) == 0) { + return; + } + + subscriptions_.erase(handle); +} diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp new file mode 100644 index 00000000..6e852be0 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -0,0 +1,85 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 DETAIL__GRAPH_CACHE_HPP_ +#define DETAIL__GRAPH_CACHE_HPP_ + +#include +#include +#include + +#include "rcutils/allocator.h" + +class PublisherData final +{ +public: + PublisherData( + const char * topic, const char * node, const char * namespace_, + const char * type, rcutils_allocator_t * allocator); + + ~PublisherData(); + +private: + rcutils_allocator_t * allocator_; + char * topic_name_{nullptr}; + char * node_name_{nullptr}; + char * namespace_name_{nullptr}; + char * type_name_{nullptr}; +}; + +class SubscriptionData final +{ +public: + SubscriptionData( + const char * topic, const char * node, const char * namespace_, + const char * type, rcutils_allocator_t * allocator); + + ~SubscriptionData(); + +private: + rcutils_allocator_t * allocator_; + char * topic_name_{nullptr}; + char * node_name_{nullptr}; + char * namespace_name_{nullptr}; + char * type_name_{nullptr}; +}; + +class GraphCache final +{ +public: + uint64_t + add_publisher( + const char * topic, const char * node, const char * namespace_, + const char * type, rcutils_allocator_t * allocator); + + void remove_publisher(uint64_t publisher_handle); + + uint64_t + add_subscription( + const char * topic, const char * node, const char * namespace_, + const char * type, rcutils_allocator_t * allocator); + + void remove_subscription(uint64_t subscription_handle); + +private: + std::mutex publishers_mutex_; + uint64_t publishers_handle_id_{0}; + std::map> publishers_; + + std::mutex subscriptions_mutex_; + uint64_t subscriptions_handle_id_{0}; + std::map> subscriptions_; +}; + +#endif // DETAIL__GRAPH_CACHE_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index b2760ce9..cc43e495 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -29,6 +29,7 @@ #include "rmw/rmw.h" +#include "graph_cache.hpp" #include "message_type_support.hpp" /// Structs for various type erased data fields. @@ -42,12 +43,18 @@ struct rmw_context_impl_s // The SHM manager. zc_owned_shm_manager_t shm_manager; + z_owned_publisher_t graph_publisher; + + z_owned_subscriber_t graph_subscriber; + /// Shutdown flag. bool is_shutdown; // Equivalent to rmw_dds_common::Context's guard condition /// Guard condition that should be triggered when the graph changes. rmw_guard_condition_t * graph_guard_condition; + + GraphCache graph_cache; }; ///============================================================================== @@ -73,6 +80,8 @@ struct rmw_publisher_data_t // Context for memory allocation for messages. rmw_context_t * context; + + uint64_t graph_cache_handle; }; ///============================================================================== @@ -119,6 +128,8 @@ struct rmw_subscription_data_t std::mutex internal_mutex; std::condition_variable * condition{nullptr}; + + uint64_t graph_cache_handle; }; #endif // DETAIL__RMW_DATA_TYPES_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index ace7da92..dfedc561 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -32,11 +32,28 @@ extern "C" { - // Megabytes of SHM to reserve. // TODO(clalancette): Make this configurable, or get it from the configuration #define SHM_BUFFER_SIZE_MB 10 +static void graph_sub_data_handler( + const z_sample_t * sample, + void * data) +{ + (void)data; + z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); + + // TODO(clalancette): Use the allocator here + char * payload_data = static_cast(malloc(sample->payload.len + 1)); + + memcpy(payload_data, sample->payload.start, sample->payload.len); + payload_data[sample->payload.len] = '\0'; + + free(payload_data); + + z_drop(z_move(keystr)); +} + //============================================================================== /// Initialize the middleware with the given options, and yielding an context. rmw_ret_t @@ -137,7 +154,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) context->impl->session = z_open(z_move(config)); if (!z_session_check(&context->impl->session)) { RMW_SET_ERROR_MSG("Error setting up zenoh session"); - return RMW_RET_INVALID_ARGUMENT; + return RMW_RET_ERROR; } auto close_session = rcpputils::make_scope_exit( [context]() { @@ -208,13 +225,46 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(gc_data->~GuardCondition(), GuardCondition); }); + auto sub_options = z_subscriber_options_default(); + sub_options.reliability = Z_RELIABILITY_RELIABLE; + z_owned_closure_sample_t callback = z_closure(graph_sub_data_handler, nullptr, context->impl); + context->impl->graph_subscriber = z_declare_subscriber( + z_loan(context->impl->session), + z_keyexpr("ros/graph/**"), + z_move(callback), + &sub_options); + auto undeclare_z_sub = rcpputils::make_scope_exit( + [context]() { + z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); + }); + if (!z_check(context->impl->graph_subscriber)) { + RMW_SET_ERROR_MSG("unable to create zenoh subscription"); + return RMW_RET_ERROR; + } + + context->impl->graph_publisher = z_declare_publisher( + z_loan(context->impl->session), + z_keyexpr("ros/graph/hello"), + NULL); + auto undeclare_z_publisher = rcpputils::make_scope_exit( + [context]() { + z_undeclare_publisher(z_move(context->impl->graph_publisher)); + }); + if (!z_check(context->impl->graph_subscriber)) { + RMW_SET_ERROR_MSG("unable to create zenoh subscription"); + return RMW_RET_ERROR; + } + + undeclare_z_publisher.cancel(); + undeclare_z_sub.cancel(); close_session.cancel(); destruct_guard_condition_data.cancel(); impl_destructor.cancel(); free_guard_condition_data.cancel(); free_guard_condition.cancel(); - free_impl.cancel(); free_options.cancel(); + impl_destructor.cancel(); + free_impl.cancel(); free_shm_manager.cancel(); restore_context.cancel(); @@ -237,6 +287,9 @@ rmw_shutdown(rmw_context_t * context) rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + z_undeclare_publisher(z_move(context->impl->graph_publisher)); + z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); + // Close the zenoh session if (z_close(z_move(context->impl->session)) < 0) { RMW_SET_ERROR_MSG("Error while closing zenoh session"); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 1c3b0af4..8cca7f85 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -136,11 +136,17 @@ rmw_create_node( allocator->deallocate(node, allocator->state); }); - node->name = rcutils_strdup(name, *allocator); + size_t name_len = strlen(name); + // We specifically don't use rcutils_strdup() here because we want to avoid iterating over the + // name again looking for the \0 (we already did that above). + char * new_string = static_cast(allocator->allocate(name_len + 1, allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - node->name, + new_string, "unable to allocate memory for node name", return nullptr); + memcpy(new_string, name, name_len); + new_string[name_len] = '\0'; + node->name = new_string; auto free_name = rcpputils::make_scope_exit( [node, allocator]() { allocator->deallocate(const_cast(node->name), allocator->state); @@ -172,6 +178,15 @@ rmw_create_node( node->implementation_identifier = rmw_zenoh_identifier; node->context = context; + // Publish to the graph that a new node is in town + z_publisher_put_options_t pub_options = z_publisher_put_options_default(); + pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); + z_publisher_put( + z_loan(context->impl->graph_publisher), + reinterpret_cast(node->name), + name_len, + &pub_options); + free_node_data.cancel(); free_namespace.cancel(); free_name.cancel(); @@ -192,7 +207,14 @@ rmw_destroy_node(rmw_node_t * node) rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - // TODO(Yadunund): Unregister with storage system. + // Publish to the graph that a node has ridden off into the sunset + z_publisher_put_options_t pub_options = z_publisher_put_options_default(); + pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); + z_publisher_put( + z_loan(node->context->impl->graph_publisher), + reinterpret_cast(node->name), + strlen(node->name), + &pub_options); rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -404,29 +426,6 @@ rmw_create_publisher( allocator->deallocate(publisher_data, allocator->state); }); - // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate - // a z_publisher_put_options struct instead of passing NULL to this function. - char * zenoh_key_name = ros_topic_name_to_zenoh_key(topic_name, allocator); - RMW_CHECK_FOR_NULL_WITH_MSG( - zenoh_key_name, - "failed to allocate memory for zenoh key name", - return nullptr); - // TODO(clalancette): What happens if the key name is a valid but empty string? - publisher_data->pub = z_declare_publisher( - z_loan(context_impl->session), - z_keyexpr(zenoh_key_name), - NULL - ); - allocator->deallocate(zenoh_key_name, allocator->state); - if (!z_check(publisher_data->pub)) { - RMW_SET_ERROR_MSG("unable to create zenoh publisher"); - return nullptr; - } - auto undeclare_z_publisher = rcpputils::make_scope_exit( - [publisher_data]() { - z_undeclare_publisher(z_move(publisher_data->pub)); - }); - publisher_data->typesupport_identifier = type_support->typesupport_identifier; publisher_data->type_support_impl = type_support->data; auto callbacks = static_cast(type_support->data); @@ -460,20 +459,67 @@ rmw_create_publisher( // TODO(yadunund): Update this. rmw_publisher->can_loan_messages = false; - rmw_publisher->topic_name = rcutils_strdup(topic_name, *allocator); + size_t topic_len = strlen(topic_name); + // We specifically don't use rcutils_strdup() here because we want to avoid iterating over the + // name again looking for the \0 (we already did that above). + char * new_string = static_cast(allocator->allocate(topic_len + 1, allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - rmw_publisher->topic_name, + new_string, "Failed to allocate topic name", return nullptr); + memcpy(new_string, topic_name, topic_len); + new_string[topic_len] = '\0'; + rmw_publisher->topic_name = new_string; auto free_topic_name = rcpputils::make_scope_exit( [rmw_publisher, allocator]() { allocator->deallocate(const_cast(rmw_publisher->topic_name), allocator->state); }); + // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate + // a z_publisher_put_options struct instead of passing NULL to this function. + char * zenoh_key_name = ros_topic_name_to_zenoh_key(topic_name, allocator); + RMW_CHECK_FOR_NULL_WITH_MSG( + zenoh_key_name, + "failed to allocate memory for zenoh key name", + return nullptr); + // TODO(clalancette): What happens if the key name is a valid but empty string? + publisher_data->pub = z_declare_publisher( + z_loan(context_impl->session), + z_keyexpr(zenoh_key_name), + NULL + ); + allocator->deallocate(zenoh_key_name, allocator->state); + if (!z_check(publisher_data->pub)) { + RMW_SET_ERROR_MSG("unable to create zenoh publisher"); + return nullptr; + } + auto undeclare_z_publisher = rcpputils::make_scope_exit( + [publisher_data]() { + z_undeclare_publisher(z_move(publisher_data->pub)); + }); + + // Publish to the graph that a new publisher is in town + z_publisher_put_options_t pub_options = z_publisher_put_options_default(); + pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); + z_publisher_put( + z_loan(node->context->impl->graph_publisher), + reinterpret_cast(rmw_publisher->topic_name), + topic_len, + &pub_options); + + publisher_data->graph_cache_handle = node->context->impl->graph_cache.add_publisher( + rmw_publisher->topic_name, node->name, node->namespace_, + publisher_data->type_support->get_name(), allocator); + auto remove_from_graph_cache = rcpputils::make_scope_exit( + [node, publisher_data]() { + node->context->impl->graph_cache.remove_publisher(publisher_data->graph_cache_handle); + }); + + remove_from_graph_cache.cancel(); + undeclare_z_publisher.cancel(); free_topic_name.cancel(); destruct_msg_type_support.cancel(); free_type_support.cancel(); - undeclare_z_publisher.cancel(); free_publisher_data.cancel(); free_rmw_publisher.cancel(); @@ -500,12 +546,23 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) rmw_ret_t ret = RMW_RET_OK; + // Publish to the graph that a publisher has ridden off into the sunset + z_publisher_put_options_t pub_options = z_publisher_put_options_default(); + pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); + z_publisher_put( + z_loan(node->context->impl->graph_publisher), + reinterpret_cast(publisher->topic_name), + strlen(publisher->topic_name), + &pub_options); + rcutils_allocator_t * allocator = &node->context->options.allocator; allocator->deallocate(const_cast(publisher->topic_name), allocator->state); auto publisher_data = static_cast(publisher->data); if (publisher_data != nullptr) { + node->context->impl->graph_cache.remove_publisher(publisher_data->graph_cache_handle); + RMW_TRY_DESTRUCTOR(publisher_data->type_support->~MessageTypeSupport(), MessageTypeSupport, ); allocator->deallocate(publisher_data->type_support, allocator->state); if (z_undeclare_publisher(z_move(publisher_data->pub))) { @@ -690,7 +747,7 @@ rmw_publish( // Returns 0 if success. ret = z_publisher_put( z_loan(publisher_data->pub), - (const uint8_t *)msg_bytes, + reinterpret_cast(msg_bytes), data_length, &options); } @@ -1062,11 +1119,17 @@ rmw_create_subscription( rmw_subscription->implementation_identifier = rmw_zenoh_identifier; rmw_subscription->data = sub_data; - rmw_subscription->topic_name = rcutils_strdup(topic_name, *allocator); + size_t topic_len = strlen(topic_name); + // We specifically don't use rcutils_strdup() here because we want to avoid iterating over the + // name again looking for the \0 (we already did that above). + char * new_string = static_cast(allocator->allocate(topic_len + 1, allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - rmw_subscription->topic_name, + new_string, "Failed to allocate topic name", return nullptr); + memcpy(new_string, topic_name, topic_len); + new_string[topic_len] = '\0'; + rmw_subscription->topic_name = new_string; auto free_topic_name = rcpputils::make_scope_exit( [rmw_subscription, allocator]() { allocator->deallocate(const_cast(rmw_subscription->topic_name), allocator->state); @@ -1102,6 +1165,24 @@ rmw_create_subscription( z_undeclare_subscriber(z_move(sub_data->sub)); }); + // Publish to the graph that a new subscription is in town + z_publisher_put_options_t pub_options = z_publisher_put_options_default(); + pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); + z_publisher_put( + z_loan(node->context->impl->graph_publisher), + reinterpret_cast(rmw_subscription->topic_name), + topic_len, + &pub_options); + + sub_data->graph_cache_handle = node->context->impl->graph_cache.add_subscription( + rmw_subscription->topic_name, node->name, node->namespace_, + sub_data->type_support->get_name(), allocator); + auto remove_from_graph_cache = rcpputils::make_scope_exit( + [node, sub_data]() { + node->context->impl->graph_cache.remove_subscription(sub_data->graph_cache_handle); + }); + + remove_from_graph_cache.cancel(); undeclare_z_sub.cancel(); free_topic_name.cancel(); destruct_msg_type_support.cancel(); @@ -1133,12 +1214,23 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) rmw_ret_t ret = RMW_RET_OK; + // Publish to the graph that a subscription has ridden off into the sunset + z_publisher_put_options_t pub_options = z_publisher_put_options_default(); + pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); + z_publisher_put( + z_loan(node->context->impl->graph_publisher), + reinterpret_cast(subscription->topic_name), + strlen(subscription->topic_name), + &pub_options); + rcutils_allocator_t * allocator = &node->context->options.allocator; allocator->deallocate(const_cast(subscription->topic_name), allocator->state); auto sub_data = static_cast(subscription->data); if (sub_data != nullptr) { + node->context->impl->graph_cache.remove_subscription(sub_data->graph_cache_handle); + RMW_TRY_DESTRUCTOR(sub_data->type_support->~MessageTypeSupport(), MessageTypeSupport, ); allocator->deallocate(sub_data->type_support, allocator->state); From 1555f7e7fbe920cfc761151787fb6dc062d81353 Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 8 Nov 2023 04:12:11 +0800 Subject: [PATCH 46/50] Add ros_domain_id as prefix to key expressions (#32) * Prefix keyexprs with the ros_domain_id Signed-off-by: Yadunund Co-authored-by: Chris Lalancette --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 59 ++++++++++++++++++++++----------- 1 file changed, 39 insertions(+), 20 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 8cca7f85..19871ac2 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" @@ -270,15 +271,17 @@ rmw_fini_publisher_allocation( return RMW_RET_UNSUPPORTED; } +//============================================================================== // A function to take ros topic names and convert them to valid Zenoh keys. // In particular, Zenoh keys cannot start or end with a /, so this function // will strip them out. +// The Zenoh key is also prefixed with the ros_domain_id. // Performance note: at present, this function allocates a new string and copies -// the old string into it. If this becomes a performance problem, we could consider -// modifying the topic_name in place. But this means we need to be much more +// the old string into it. If this becomes a performance problem, we could consider +// modifying the topic_name in place. But this means we need to be much more // careful about who owns the string. -static char * ros_topic_name_to_zenoh_key( - const char * const topic_name, rcutils_allocator_t * allocator) +static z_owned_keyexpr_t ros_topic_name_to_zenoh_key( + const char * const topic_name, size_t domain_id, rcutils_allocator_t * allocator) { size_t start_offset = 0; size_t topic_name_len = strlen(topic_name); @@ -295,9 +298,18 @@ static char * ros_topic_name_to_zenoh_key( } } - return rcutils_strndup(&topic_name[start_offset], end_offset - start_offset, *allocator); + std::stringstream domain_ss; + domain_ss << domain_id; + char * stripped_topic_name = rcutils_strndup( + &topic_name[start_offset], end_offset - start_offset, *allocator); + z_owned_keyexpr_t keyexpr = z_keyexpr_join( + z_keyexpr(domain_ss.str().c_str()), z_keyexpr(stripped_topic_name)); + allocator->deallocate(stripped_topic_name, allocator->state); + + return keyexpr; } +//============================================================================== static const rosidl_message_type_support_t * find_type_support( const rosidl_message_type_support_t * type_supports) { @@ -477,18 +489,22 @@ rmw_create_publisher( // TODO(yadunund): Parse adapted_qos_profile and publisher_options to generate // a z_publisher_put_options struct instead of passing NULL to this function. - char * zenoh_key_name = ros_topic_name_to_zenoh_key(topic_name, allocator); - RMW_CHECK_FOR_NULL_WITH_MSG( - zenoh_key_name, - "failed to allocate memory for zenoh key name", - return nullptr); + z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key( + topic_name, node->context->actual_domain_id, allocator); + auto always_free_ros_keyexpr = rcpputils::make_scope_exit( + [&keyexpr]() { + z_keyexpr_drop(z_move(keyexpr)); + }); + if (!z_keyexpr_check(&keyexpr)) { + RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); + return nullptr; + } // TODO(clalancette): What happens if the key name is a valid but empty string? publisher_data->pub = z_declare_publisher( z_loan(context_impl->session), - z_keyexpr(zenoh_key_name), + z_loan(keyexpr), NULL ); - allocator->deallocate(zenoh_key_name, allocator->state); if (!z_check(publisher_data->pub)) { RMW_SET_ERROR_MSG("unable to create zenoh publisher"); return nullptr; @@ -1143,19 +1159,22 @@ rmw_create_subscription( // with Zenoh; after this, callbacks may come in at any time. z_owned_closure_sample_t callback = z_closure(sub_data_handler, nullptr, sub_data); - char * zenoh_key_name = ros_topic_name_to_zenoh_key(topic_name, allocator); - // TODO(clalancette): What happens if the key name is a valid but empty string? - RMW_CHECK_FOR_NULL_WITH_MSG( - zenoh_key_name, - "failed to allocate memory for zenoh key name", - return nullptr); + z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key( + topic_name, node->context->actual_domain_id, allocator); + auto always_free_ros_keyexpr = rcpputils::make_scope_exit( + [&keyexpr]() { + z_keyexpr_drop(z_move(keyexpr)); + }); + if (!z_keyexpr_check(&keyexpr)) { + RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); + return nullptr; + } sub_data->sub = z_declare_subscriber( z_loan(context_impl->session), - z_keyexpr(zenoh_key_name), + z_loan(keyexpr), z_move(callback), &sub_options ); - allocator->deallocate(zenoh_key_name, allocator->state); if (!z_check(sub_data->sub)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; From 3ed081bc2ccf53518280a582b0649c607a683309 Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Wed, 8 Nov 2023 11:11:41 -0300 Subject: [PATCH 47/50] Adds documentation about default topology being used. (#30) Signed-off-by: Franco Cipollone --- README.md | 4 ++++ docs/design.md | 58 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 62 insertions(+) create mode 100644 docs/design.md diff --git a/README.md b/README.md index 102401d2..a81c3583 100644 --- a/README.md +++ b/README.md @@ -5,6 +5,10 @@ A ROS 2 RMW implementation based on Zenoh that is written using the zenoh-c bindings. +## Design + +For information about the Design please visit [design](docs/design.md) page. + ## Requirements - [ROS 2](https://docs.ros.org): Rolling/Iron diff --git a/docs/design.md b/docs/design.md new file mode 100644 index 00000000..91cfc014 --- /dev/null +++ b/docs/design.md @@ -0,0 +1,58 @@ +# Design + +## Network Topology + +The following diagram shows the default network topology of a subsystem composed of 3 nodes: + +```mermaid +flowchart TB + %% COLORS %% + classDef blue fill:#2374f7,stroke:#000,stroke-width:2px,color:#fff + classDef red fill:#ed2633,stroke:#000,stroke-width:2px,color:#fff + classDef green fill:#16b522,stroke:#000,stroke-width:2px,color:#fff + classDef orange fill:#fc822b,stroke:#000,stroke-width:2px,color:#fff + classDef purple fill:#a724f7,stroke:#000,stroke-width:2px,color:#fff + classDef yellow fill:#a724f7,stroke:#000,stroke-width:2px,color:#fff + + %% DIAGRAM %% + Router(Zenoh Router:\n tcp/localhost:7447):::yellow + + %% Discovery connections %% + + Router <-..-> |discovery| S1(["Zenoh Session\n(Pub)"]):::blue + Router <-..-> |discovery| S2(["Zenoh Session\n(Sub)"]):::blue + Router <-..-> |discovery| S3(["Zenoh Session\n(Sub)"]):::blue + + subgraph Sessions + + %% P2P connections %% + S1 <--> |p2p| S2 + S1 <--> |p2p| S3 + S2 <--> |p2p| S3 + + linkStyle 3 stroke:red + linkStyle 4 stroke:red + linkStyle 5 stroke:red + + %% Data connections %% + S1 --> |Data| S2 + S1 --> |Data| S3 + + linkStyle 6 stroke:green + linkStyle 7 stroke:green + end +``` +Default Configuration for Zenoh Sessions: +| Config | Zenoh Session | Zenoh Router | +| :---: | :---: | :---: | +| Mode | Peer | Router | +| Connect | tcp/localhost:7447 | - | +| UDP Multicast | Disabled | Disabled | +| Gossip Scouting | Enabled | Enabled | + +### Notes + + - Zenoh routers (a.k.a. `zenohd`) are required mainly to allow the peers to discover each other within the subsystem. + - Zenoh sessions are configured in `peer` mode and connect directly to the router looking for other peers. As `gossip` scouting is being used, the router is in charge to spread the discovery information across the peers of the subsystem. + - Each Zenoh sessions discover each other and creates direct `Peer-To-Peer` connection between them and the connection remains without relying on the router. + - By means of the `Peer-To-Peer` connection, the publisher-subscriber interaction happens: Data flows from the publishers to the subscribers. From c5dcad8256824ca45ce0ac1027b14b34bf2c0cdb Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Wed, 8 Nov 2023 11:43:04 -0300 Subject: [PATCH 48/50] Adds ZenohConfig class to handle z_config setup. (#33) * Adds ZenohConfig class to handle z_config setup. Signed-off-by: Franco Cipollone Co-authored-by: Chris Lalancette --- README.md | 13 +- rmw_zenoh_cpp/CMakeLists.txt | 8 + .../DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 | 261 ++++++++++++++++++ rmw_zenoh_cpp/package.xml | 1 + rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 76 +++++ rmw_zenoh_cpp/src/detail/zenoh_config.hpp | 34 +++ rmw_zenoh_cpp/src/rmw_init.cpp | 21 +- 7 files changed, 397 insertions(+), 17 deletions(-) create mode 100644 rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 create mode 100644 rmw_zenoh_cpp/src/detail/zenoh_config.cpp create mode 100644 rmw_zenoh_cpp/src/detail/zenoh_config.hpp diff --git a/README.md b/README.md index a81c3583..7bed7356 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,18 @@ ros2 topic pub "/chatter" std_msgs/msg/String '{data: hello}' ``` ## Config -The zenoh session may be configured by setting the `ZENOH_CONFIG_PATH` environment variable to point to the custom zenoh configuration file. The [rmw_zenoh_config.json5](./rmw_zenoh_config.json5) may be used to configure the zenoh session for this middleware. For a complete list of configurable settings, see [documentation](https://github.com/eclipse-zenoh/zenoh/blob/master/DEFAULT_CONFIG.json5). +The [default configuration](rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5) sets up the zenoh sessions with the following main characteristics: + +Table: +| Zenoh Config | Default | +| :---: | :---: | +| udp_multicast | disabled | +| gossip scouting | enabled | +| connect | tcp/localhost:7447 | + +This assumes that there is a `zenohd` running in the system at port 7447. +A custom configuration may be provided by setting the `RMW_ZENOH_CONFIG_FILE` environment variable to point to a custom zenoh configuration file. + ## TODO Features - [x] Publisher diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index bce95ece..5236d785 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -14,6 +14,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) find_package(fastcdr CONFIG REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) @@ -32,6 +33,7 @@ add_library(rmw_zenoh_cpp SHARED src/detail/service_type_support.cpp src/detail/type_support.cpp src/detail/type_support_common.cpp + src/detail/zenoh_config.cpp src/rmw_event.cpp src/rmw_get_network_flow_endpoints.cpp src/rmw_get_node_info_and_types.cpp @@ -46,6 +48,7 @@ add_library(rmw_zenoh_cpp SHARED target_link_libraries(rmw_zenoh_cpp PRIVATE + ament_index_cpp::ament_index_cpp fastcdr rcpputils::rcpputils rcutils::rcutils @@ -83,4 +86,9 @@ install( RUNTIME DESTINATION bin ) +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + ament_package() diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 new file mode 100644 index 00000000..c1418252 --- /dev/null +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 @@ -0,0 +1,261 @@ +/// This file attempts to list and document available configuration elements. +/// For a more complete view of the configuration's structure, check out `zenoh/src/config.rs`'s `Config` structure. +/// Note that the values here are correctly typed, but may not be sensible, so copying this file to change only the parts that matter to you is not good practice. +{ + /// The identifier (as unsigned 128bit integer in hexadecimal lowercase - leading zeros are not accepted) + /// that zenoh runtime will use. + /// If not set, a random unsigned 128bit integer will be used. + /// WARNING: this id must be unique in your zenoh network. + // id: "1234567890abcdef", + + /// The node's mode (router, peer or client) + mode: "peer", + + /// Which endpoints to connect to. E.g. tcp/localhost:7447. + /// By configuring the endpoints, it is possible to tell zenoh which router/peer to connect to at startup. + connect: { + endpoints: [ + "tcp/localhost:7447", + ], + }, + + /// Which endpoints to listen on. E.g. tcp/localhost:7447. + /// By configuring the endpoints, it is possible to tell zenoh which are the endpoints that other routers, + /// peers, or client can use to establish a zenoh session. + listen: { + endpoints: [ + // "tcp/localhost:7447", + ], + }, + /// Configure the scouting mechanisms and their behaviours + scouting: { + /// In client mode, the period dedicated to scouting for a router before failing + timeout: 3000, + /// In peer mode, the period dedicated to scouting remote peers before attempting other operations + delay: 1, + /// The multicast scouting configuration. + multicast: { + /// Whether multicast scouting is enabled or not + enabled: false, + /// The socket which should be used for multicast scouting + address: "224.0.0.224:7446", + /// The network interface which should be used for multicast scouting + interface: "auto", // If not set or set to "auto" the interface if picked automatically + /// Which type of Zenoh instances to automatically establish sessions with upon discovery on UDP multicast. + /// Accepts a single value or different values for router, peer and client. + /// Each value is bit-or-like combinations of "peer", "router" and "client". + autoconnect: { router: "", peer: "router|peer" }, + /// Whether or not to listen for scout messages on UDP multicast and reply to them. + listen: true, + }, + /// The gossip scouting configuration. + gossip: { + /// Whether gossip scouting is enabled or not + enabled: true, + /// When true, gossip scouting informations are propagated multiple hops to all nodes in the local network. + /// When false, gossip scouting informations are only propagated to the next hop. + /// Activating multihop gossip implies more scouting traffic and a lower scalability. + /// It mostly makes sense when using "linkstate" routing mode where all nodes in the subsystem don't have + /// direct connectivity with each other. + multihop: false, + /// Which type of Zenoh instances to automatically establish sessions with upon discovery on gossip. + /// Accepts a single value or different values for router, peer and client. + /// Each value is bit-or-like combinations of "peer", "router" and "client". + autoconnect: { router: "", peer: "router|peer" }, + }, + }, + + /// Configuration of data messages timestamps management. + timestamping: { + /// Whether data messages should be timestamped if not already. + /// Accepts a single boolean value or different values for router, peer and client. + enabled: { router: true, peer: false, client: false }, + /// Whether data messages with timestamps in the future should be dropped or not. + /// If set to false (default), messages with timestamps in the future are retimestamped. + /// Timestamps are ignored if timestamping is disabled. + drop_future_timestamp: false, + }, + + /// The default timeout to apply to queries in milliseconds. + queries_default_timeout: 10000, + + /// The routing strategy to use and it's configuration. + routing: { + /// The routing strategy to use in routers and it's configuration. + router: { + /// When set to true a router will forward data between two peers + /// directly connected to it if it detects that those peers are not + /// connected to each other. + /// The failover brokering only works if gossip discovery is enabled. + peers_failover_brokering: true, + }, + /// The routing strategy to use in peers and it's configuration. + peer: { + /// The routing strategy to use in peers. ("peer_to_peer" or "linkstate"). + mode: "peer_to_peer", + }, + }, + + // /// The declarations aggregation strategy. + // aggregation: { + // /// A list of key-expressions for which all included subscribers will be aggregated into. + // subscribers: [ + // // key_expression + // ], + // /// A list of key-expressions for which all included publishers will be aggregated into. + // publishers: [ + // // key_expression + // ], + // }, + + /// Configure internal transport parameters + transport: { + unicast: { + /// Timeout in milliseconds when opening a link + accept_timeout: 10000, + /// Maximum number of zenoh session in pending state while accepting + accept_pending: 100, + /// Maximum number of sessions that can be simultaneously alive + max_sessions: 1000, + /// Maximum number of incoming links that are admitted per session + max_links: 1, + /// Enables the LowLatency transport + /// This option does not make LowLatency transport mandatory, the actual implementation of transport + /// used will depend on Establish procedure and other party's settings + /// + /// NOTE: Currently, the LowLatency transport doesn't preserve QoS prioritization. + /// NOTE: Due to the note above, 'lowlatency' is incompatible with 'qos' option, so in order to + /// enable 'lowlatency' you need to explicitly disable 'qos'. + lowlatency: false, + }, + qos: { + enabled: true, + }, + link: { + /// An optional whitelist of protocols to be used for accepting and opening sessions. + /// If not configured, all the supported protocols are automatically whitelisted. + /// The supported protocols are: ["tcp" , "udp", "tls", "quic", "ws", "unixsock-stream"] + /// For example, to only enable "tls" and "quic": + // protocols: ["tls", "quic"], + /// Configure the zenoh TX parameters of a link + tx: { + /// The resolution in bits to be used for the message sequence numbers. + /// When establishing a session with another Zenoh instance, the lowest value of the two instances will be used. + /// Accepted values: 8bit, 16bit, 32bit, 64bit. + sequence_number_resolution: "32bit", + /// Link lease duration in milliseconds to announce to other zenoh nodes + lease: 10000, + /// Number of keep-alive messages in a link lease duration. If no data is sent, keep alive + /// messages will be sent at the configured time interval. + /// NOTE: In order to consider eventual packet loss and transmission latency and jitter, + /// set the actual keep_alive timeout to one fourth of the lease time. + /// This is in-line with the ITU-T G.8013/Y.1731 specification on continous connectivity + /// check which considers a link as failed when no messages are received in 3.5 times the + /// target interval. + keep_alive: 4, + /// Batch size in bytes is expressed as a 16bit unsigned integer. + /// Therefore, the maximum batch size is 2^16-1 (i.e. 65535). + /// The default batch size value is the maximum batch size: 65535. + batch_size: 65535, + /// Each zenoh link has a transmission queue that can be configured + queue: { + /// The size of each priority queue indicates the number of batches a given queue can contain. + /// The amount of memory being allocated for each queue is then SIZE_XXX * BATCH_SIZE. + /// In the case of the transport link MTU being smaller than the ZN_BATCH_SIZE, + /// then amount of memory being allocated for each queue is SIZE_XXX * LINK_MTU. + /// If qos is false, then only the DATA priority will be allocated. + size: { + control: 1, + real_time: 1, + interactive_high: 1, + interactive_low: 1, + data_high: 2, + data: 4, + data_low: 4, + background: 4, + }, + /// The initial exponential backoff time in nanoseconds to allow the batching to eventually progress. + /// Higher values lead to a more aggressive batching but it will introduce additional latency. + backoff: 100, + }, + }, + /// Configure the zenoh RX parameters of a link + rx: { + /// Receiving buffer size in bytes for each link + /// The default the rx_buffer_size value is the same as the default batch size: 65335. + /// For very high throughput scenarios, the rx_buffer_size can be increased to accomodate + /// more in-flight data. This is particularly relevant when dealing with large messages. + /// E.g. for 16MiB rx_buffer_size set the value to: 16777216. + buffer_size: 65535, + /// Maximum size of the defragmentation buffer at receiver end. + /// Fragmented messages that are larger than the configured size will be dropped. + /// The default value is 1GiB. This would work in most scenarios. + /// NOTE: reduce the value if you are operating on a memory constrained device. + max_message_size: 1073741824, + }, + /// Configure TLS specific parameters + tls: { + /// Path to the certificate of the certificate authority used to validate either the server + /// or the client's keys and certificates, depending on the node's mode. If not specified + /// on router mode then the default WebPKI certificates are used instead. + root_ca_certificate: null, + /// Path to the TLS server private key + server_private_key: null, + /// Path to the TLS server public certificate + server_certificate: null, + /// Client authentication, if true enables mTLS (mutual authentication) + client_auth: false, + /// Path to the TLS client private key + client_private_key: null, + /// Path to the TLS client public certificate + client_certificate: null, + // Whether or not to use server name verification, if set to false zenoh will disregard the common names of the certificates when verifying servers. + // This could be dangerous because your CA can have signed a server cert for foo.com, that's later being used to host a server at baz.com. If you wan't your + // ca to verify that the server at baz.com is actually baz.com, let this be true (default). + server_name_verification: null, + }, + + /// **Experimental** compression feature. + /// Will compress the batches hop to hop (as opposed to end to end). + /// The features "transport_compression" and "unstable" need to be enabled to handle + /// compression on the integrality of the network. + compression: { + /// When 'enabled' is true, batches will be sent compressed. + enabled: false, + }, + }, + /// Shared memory configuration + shared_memory: { + enabled: true, // + }, + /// Access control configuration + auth: { + /// The configuration of authentification. + /// A password implies a username is required. + usrpwd: { + user: null, + password: null, + /// The path to a file containing the user password dictionary + dictionary_file: null, + }, + pubkey: { + public_key_pem: null, + private_key_pem: null, + public_key_file: null, + private_key_file: null, + key_size: null, + known_keys_file: null, + }, + }, + }, + + /// Configure the Admin Space + /// Unstable: this configuration part works as advertised, but may change in a future release + adminspace: { + // read and/or write permissions on the admin space + permissions: { + read: true, + write: false, + }, + }, +} diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index 7d27fe9c..a53ba6cc 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -11,6 +11,7 @@ zenoh_c_vendor + ament_index_cpp fastcdr rcpputils rcutils diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp new file mode 100644 index 00000000..d5880515 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -0,0 +1,76 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 "zenoh_config.hpp" + +#include +#include + +#include + +#include +#include + +#include "identifier.hpp" + +namespace +{ + +/// Env var to set the path to the zenoh configuration file. +static constexpr const char * kZenohConfigFileEnvVar = "RMW_ZENOH_CONFIG_FILE"; +/// The name of the default configuration file for the default zenoh session configuration. +static constexpr const char * const kDefaultZenohConfigFileName = + "DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5"; + +} // namespace + +rmw_ret_t get_z_config(z_owned_config_t * config) +{ + const char * zenoh_config_path; + // Get the path to the zenoh configuration file from the environment variable. + if (NULL != rcutils_get_env(kZenohConfigFileEnvVar, &zenoh_config_path)) { + // NULL is returned if everything is ok. + RCUTILS_LOG_ERROR_NAMED( + "ZenohConfiguration", "Env Var '%s' can't be read.", kZenohConfigFileEnvVar); + return RMW_RET_ERROR; + } + + // If the environment variable contains a path to a file, try to read the configuration from it. + if (zenoh_config_path[0] != '\0') { + // If the environment variable is set, try to read the configuration from the file. + *config = zc_config_from_file(zenoh_config_path); + RCUTILS_LOG_INFO_NAMED( + "ZenohConfiguration", + "Using zenoh configuration file pointed by '%s' envar: '%s'", kZenohConfigFileEnvVar, + zenoh_config_path); + } else { + // If the environment variable is not set use internal configuration + static const std::string path_to_config_folder = + ament_index_cpp::get_package_share_directory(rmw_zenoh_identifier) + "/config/"; + const std::string default_zconfig_path = path_to_config_folder + kDefaultZenohConfigFileName; + *config = zc_config_from_file(default_zconfig_path.c_str()); + RCUTILS_LOG_INFO_NAMED( + "ZenohConfiguration", + "Using default zenoh configuration file at '%s'", default_zconfig_path.c_str()); + } + + // Verify that the configuration is valid. + if (!z_config_check(config)) { + RCUTILS_LOG_ERROR_NAMED( + "ZenohConfiguration", + "Configuration is not valid. Please check the zenoh configuration file."); + return RMW_RET_ERROR; + } + return RMW_RET_OK; +} diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp new file mode 100644 index 00000000..088a1757 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -0,0 +1,34 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 DETAIL__ZENOH_CONFIG_HPP_ +#define DETAIL__ZENOH_CONFIG_HPP_ + +#include + +#include "rmw/ret_types.h" + +/// Get the zenoh configuration for a session. +/// @details The behavior is as follows: +/// - If the environment variable `RMW_ZENOH_CONFIG_FILE` is set, it will +/// be used as the path to the zenoh configuration file. +/// - In case there is an error reading the file, the internal configuration will be used. +/// - If the environment variable `RMW_ZENOH_CONFIG_FILE` is not set, the +/// configuration will be read from the internal configuration. +/// - If internal configuration is not available, a zenoh default configuration is used. +/// @param config The zenoh configuration to be filled. +/// @returns `RMW_RET_OK` if the configuration was successfully loaded. +rmw_ret_t get_z_config(z_owned_config_t * config); + +#endif // DETAIL__ZENOH_CONFIG_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index dfedc561..228c231e 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -19,6 +19,7 @@ #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" #include "detail/rmw_data_types.hpp" +#include "detail/zenoh_config.hpp" #include "rcutils/env.h" #include "rcutils/strdup.h" @@ -124,23 +125,11 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // Initialize context's implementation context->impl->is_shutdown = false; - // Initialize the zenoh config. We use the default config if a path to the - // config is unavailable. + // Initialize the zenoh configuration. z_owned_config_t config; - const char * zenoh_config_path; - if (NULL != rcutils_get_env("ZENOH_CONFIG_PATH", &zenoh_config_path)) { - RMW_SET_ERROR_MSG("Error in reading ZENOH_CONFIG_PATH envar"); - return RMW_RET_INVALID_ARGUMENT; - } - if (zenoh_config_path[0] == '\0') { - // No config path set. - config = z_config_default(); - } else { - config = zc_config_from_file(zenoh_config_path); - if (!z_config_check(&config)) { - RMW_SET_ERROR_MSG("Error in zenoh config path"); - return RMW_RET_INVALID_ARGUMENT; - } + if ((ret = get_z_config(&config)) != RMW_RET_OK) { + RMW_SET_ERROR_MSG("Error setting up zenoh configuration for the session."); + return ret; } // Check if shm is enabled. From 1f368c3ccc043164166fe6c9cb54f78068c6611c Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Wed, 8 Nov 2023 12:28:46 -0300 Subject: [PATCH 49/50] Provides zenoh router configuration and executable. (#34) * Provides zenoh router configuration and executable. * Remove custom hooks. Just install init_rmw_zenoh_router so that ros2 run can find it. Signed-off-by: Franco Cipollone Co-authored-by: Chris Lalancette --- README.md | 20 ++ rmw_zenoh_cpp/CMakeLists.txt | 12 + rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp | 45 +++ .../DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 | 261 ++++++++++++++++++ rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 2 +- rmw_zenoh_cpp/src/rmw_init.cpp | 5 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 7 +- 7 files changed, 345 insertions(+), 7 deletions(-) create mode 100644 rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp create mode 100644 rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 diff --git a/README.md b/README.md index 7bed7356..09b04845 100644 --- a/README.md +++ b/README.md @@ -21,6 +21,13 @@ Install latest rustc. curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh ``` +Install zenohd router +> Note: The manual zenoh router installation won't be required in the future. +```bash +echo "deb [trusted=yes] https://download.eclipse.org/zenoh/debian-repo/ /" | sudo tee -a /etc/apt/sources.list > /dev/null +sudo apt update && sudo apt install zenoh -y +``` + Build `rmw_zenoh_cpp` ```bash @@ -33,9 +40,22 @@ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release ``` ## Test + +Source workspace ```bash cd ~/ws_rmw_alternative source install/setup.bash +``` + +In a terminal launch Zenoh router: +```bash +ros2 run rmw_zenoh_cpp init_rmw_zenoh_router +``` +> Note: Manually launching zenoh router won't be necessary in the future. + +In a different terminal source install folder and execute: + +```bash export RMW_IMPLEMENTATION=rmw_zenoh_cpp ros2 topic pub "/chatter" std_msgs/msg/String '{data: hello}' ``` diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 5236d785..a2b4bc5e 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -91,4 +91,16 @@ install( DESTINATION share/${PROJECT_NAME} ) +add_executable(init_rmw_zenoh_router apps/init_rmw_zenoh_router.cpp) + +target_link_libraries(init_rmw_zenoh_router + PRIVATE + ament_index_cpp::ament_index_cpp +) + +install( + TARGETS init_rmw_zenoh_router + DESTINATION lib/${PROJECT_NAME} +) + ament_package() diff --git a/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp b/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp new file mode 100644 index 00000000..30b67501 --- /dev/null +++ b/rmw_zenoh_cpp/apps/init_rmw_zenoh_router.cpp @@ -0,0 +1,45 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// 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 + +namespace rmw_zenoh_cpp +{ + +int Main(int, char **) +{ + static const char * RMW_ZENOH_IDENTIFIER = "rmw_zenoh_cpp"; + static const char * ZENOH_ROUTER_CONFIG_NAME = "DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5"; + const std::string zenoh_router_config_path = + ament_index_cpp::get_package_share_directory(RMW_ZENOH_IDENTIFIER) + + "/config/" + std::string(ZENOH_ROUTER_CONFIG_NAME); + + // Execute zenohd command + const std::string zenohd_cmd = "zenohd -c " + zenoh_router_config_path; + const int ret = system(zenohd_cmd.c_str()); + if (ret != 0) { + std::cerr << "Failed to run zenoh router: " << zenohd_cmd << std::endl; + return ret; + } + return 0; +} + +} // namespace rmw_zenoh_cpp + +int main(int argc, char ** argv) {return rmw_zenoh_cpp::Main(argc, argv);} diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 new file mode 100644 index 00000000..3fa61761 --- /dev/null +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 @@ -0,0 +1,261 @@ +/// This file attempts to list and document available configuration elements. +/// For a more complete view of the configuration's structure, check out `zenoh/src/config.rs`'s `Config` structure. +/// Note that the values here are correctly typed, but may not be sensible, so copying this file to change only the parts that matter to you is not good practice. +{ + /// The identifier (as unsigned 128bit integer in hexadecimal lowercase - leading zeros are not accepted) + /// that zenoh runtime will use. + /// If not set, a random unsigned 128bit integer will be used. + /// WARNING: this id must be unique in your zenoh network. + // id: "1234567890abcdef", + + /// The node's mode (router, peer or client) + mode: "router", + + /// Which endpoints to connect to. E.g. tcp/localhost:7447. + /// By configuring the endpoints, it is possible to tell zenoh which router/peer to connect to at startup. + connect: { + endpoints: [ + // "/
" + ], + }, + + /// Which endpoints to listen on. E.g. tcp/localhost:7447. + /// By configuring the endpoints, it is possible to tell zenoh which are the endpoints that other routers, + /// peers, or client can use to establish a zenoh session. + listen: { + endpoints: [ + "tcp/localhost:7447" + ], + }, + /// Configure the scouting mechanisms and their behaviours + scouting: { + /// In client mode, the period dedicated to scouting for a router before failing + timeout: 3000, + /// In peer mode, the period dedicated to scouting remote peers before attempting other operations + delay: 1, + /// The multicast scouting configuration. + multicast: { + /// Whether multicast scouting is enabled or not + enabled: false, + /// The socket which should be used for multicast scouting + address: "224.0.0.224:7446", + /// The network interface which should be used for multicast scouting + interface: "auto", // If not set or set to "auto" the interface if picked automatically + /// Which type of Zenoh instances to automatically establish sessions with upon discovery on UDP multicast. + /// Accepts a single value or different values for router, peer and client. + /// Each value is bit-or-like combinations of "peer", "router" and "client". + autoconnect: { router: "", peer: "router|peer" }, + /// Whether or not to listen for scout messages on UDP multicast and reply to them. + listen: true, + }, + /// The gossip scouting configuration. + gossip: { + /// Whether gossip scouting is enabled or not + enabled: true, + /// When true, gossip scouting informations are propagated multiple hops to all nodes in the local network. + /// When false, gossip scouting informations are only propagated to the next hop. + /// Activating multihop gossip implies more scouting traffic and a lower scalability. + /// It mostly makes sense when using "linkstate" routing mode where all nodes in the subsystem don't have + /// direct connectivity with each other. + multihop: false, + /// Which type of Zenoh instances to automatically establish sessions with upon discovery on gossip. + /// Accepts a single value or different values for router, peer and client. + /// Each value is bit-or-like combinations of "peer", "router" and "client". + autoconnect: { router: "", peer: "router|peer" }, + }, + }, + + /// Configuration of data messages timestamps management. + timestamping: { + /// Whether data messages should be timestamped if not already. + /// Accepts a single boolean value or different values for router, peer and client. + enabled: { router: true, peer: false, client: false }, + /// Whether data messages with timestamps in the future should be dropped or not. + /// If set to false (default), messages with timestamps in the future are retimestamped. + /// Timestamps are ignored if timestamping is disabled. + drop_future_timestamp: false, + }, + + /// The default timeout to apply to queries in milliseconds. + queries_default_timeout: 10000, + + /// The routing strategy to use and it's configuration. + routing: { + /// The routing strategy to use in routers and it's configuration. + router: { + /// When set to true a router will forward data between two peers + /// directly connected to it if it detects that those peers are not + /// connected to each other. + /// The failover brokering only works if gossip discovery is enabled. + peers_failover_brokering: true, + }, + /// The routing strategy to use in peers and it's configuration. + peer: { + /// The routing strategy to use in peers. ("peer_to_peer" or "linkstate"). + mode: "peer_to_peer", + }, + }, + + // /// The declarations aggregation strategy. + // aggregation: { + // /// A list of key-expressions for which all included subscribers will be aggregated into. + // subscribers: [ + // // key_expression + // ], + // /// A list of key-expressions for which all included publishers will be aggregated into. + // publishers: [ + // // key_expression + // ], + // }, + + /// Configure internal transport parameters + transport: { + unicast: { + /// Timeout in milliseconds when opening a link + accept_timeout: 10000, + /// Maximum number of zenoh session in pending state while accepting + accept_pending: 100, + /// Maximum number of sessions that can be simultaneously alive + max_sessions: 1000, + /// Maximum number of incoming links that are admitted per session + max_links: 1, + /// Enables the LowLatency transport + /// This option does not make LowLatency transport mandatory, the actual implementation of transport + /// used will depend on Establish procedure and other party's settings + /// + /// NOTE: Currently, the LowLatency transport doesn't preserve QoS prioritization. + /// NOTE: Due to the note above, 'lowlatency' is incompatible with 'qos' option, so in order to + /// enable 'lowlatency' you need to explicitly disable 'qos'. + lowlatency: false, + }, + qos: { + enabled: true, + }, + link: { + /// An optional whitelist of protocols to be used for accepting and opening sessions. + /// If not configured, all the supported protocols are automatically whitelisted. + /// The supported protocols are: ["tcp" , "udp", "tls", "quic", "ws", "unixsock-stream"] + /// For example, to only enable "tls" and "quic": + // protocols: ["tls", "quic"], + /// Configure the zenoh TX parameters of a link + tx: { + /// The resolution in bits to be used for the message sequence numbers. + /// When establishing a session with another Zenoh instance, the lowest value of the two instances will be used. + /// Accepted values: 8bit, 16bit, 32bit, 64bit. + sequence_number_resolution: "32bit", + /// Link lease duration in milliseconds to announce to other zenoh nodes + lease: 10000, + /// Number of keep-alive messages in a link lease duration. If no data is sent, keep alive + /// messages will be sent at the configured time interval. + /// NOTE: In order to consider eventual packet loss and transmission latency and jitter, + /// set the actual keep_alive timeout to one fourth of the lease time. + /// This is in-line with the ITU-T G.8013/Y.1731 specification on continous connectivity + /// check which considers a link as failed when no messages are received in 3.5 times the + /// target interval. + keep_alive: 4, + /// Batch size in bytes is expressed as a 16bit unsigned integer. + /// Therefore, the maximum batch size is 2^16-1 (i.e. 65535). + /// The default batch size value is the maximum batch size: 65535. + batch_size: 65535, + /// Each zenoh link has a transmission queue that can be configured + queue: { + /// The size of each priority queue indicates the number of batches a given queue can contain. + /// The amount of memory being allocated for each queue is then SIZE_XXX * BATCH_SIZE. + /// In the case of the transport link MTU being smaller than the ZN_BATCH_SIZE, + /// then amount of memory being allocated for each queue is SIZE_XXX * LINK_MTU. + /// If qos is false, then only the DATA priority will be allocated. + size: { + control: 1, + real_time: 1, + interactive_high: 1, + interactive_low: 1, + data_high: 2, + data: 4, + data_low: 4, + background: 4, + }, + /// The initial exponential backoff time in nanoseconds to allow the batching to eventually progress. + /// Higher values lead to a more aggressive batching but it will introduce additional latency. + backoff: 100, + }, + }, + /// Configure the zenoh RX parameters of a link + rx: { + /// Receiving buffer size in bytes for each link + /// The default the rx_buffer_size value is the same as the default batch size: 65335. + /// For very high throughput scenarios, the rx_buffer_size can be increased to accomodate + /// more in-flight data. This is particularly relevant when dealing with large messages. + /// E.g. for 16MiB rx_buffer_size set the value to: 16777216. + buffer_size: 65535, + /// Maximum size of the defragmentation buffer at receiver end. + /// Fragmented messages that are larger than the configured size will be dropped. + /// The default value is 1GiB. This would work in most scenarios. + /// NOTE: reduce the value if you are operating on a memory constrained device. + max_message_size: 1073741824, + }, + /// Configure TLS specific parameters + tls: { + /// Path to the certificate of the certificate authority used to validate either the server + /// or the client's keys and certificates, depending on the node's mode. If not specified + /// on router mode then the default WebPKI certificates are used instead. + root_ca_certificate: null, + /// Path to the TLS server private key + server_private_key: null, + /// Path to the TLS server public certificate + server_certificate: null, + /// Client authentication, if true enables mTLS (mutual authentication) + client_auth: false, + /// Path to the TLS client private key + client_private_key: null, + /// Path to the TLS client public certificate + client_certificate: null, + // Whether or not to use server name verification, if set to false zenoh will disregard the common names of the certificates when verifying servers. + // This could be dangerous because your CA can have signed a server cert for foo.com, that's later being used to host a server at baz.com. If you wan't your + // ca to verify that the server at baz.com is actually baz.com, let this be true (default). + server_name_verification: null, + }, + + /// **Experimental** compression feature. + /// Will compress the batches hop to hop (as opposed to end to end). + /// The features "transport_compression" and "unstable" need to be enabled to handle + /// compression on the integrality of the network. + compression: { + /// When 'enabled' is true, batches will be sent compressed. + enabled: false, + }, + }, + /// Shared memory configuration + shared_memory: { + enabled: false, + }, + /// Access control configuration + auth: { + /// The configuration of authentification. + /// A password implies a username is required. + usrpwd: { + user: null, + password: null, + /// The path to a file containing the user password dictionary + dictionary_file: null, + }, + pubkey: { + public_key_pem: null, + private_key_pem: null, + public_key_file: null, + private_key_file: null, + key_size: null, + known_keys_file: null, + }, + }, + }, + + /// Configure the Admin Space + /// Unstable: this configuration part works as advertised, but may change in a future release + adminspace: { + // read and/or write permissions on the admin space + permissions: { + read: true, + write: false, + }, + }, +} diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index d5880515..dd799dfe 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -33,7 +33,7 @@ static constexpr const char * kZenohConfigFileEnvVar = "RMW_ZENOH_CONFIG_FILE"; static constexpr const char * const kDefaultZenohConfigFileName = "DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5"; -} // namespace +} // namespace rmw_ret_t get_z_config(z_owned_config_t * config) { diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 228c231e..08e98792 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -156,10 +156,11 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) { z_id_t id = z_info_zid(z_loan(context->impl->session)); char idstr[sizeof(id.id) * 2 + 1]; // 2 bytes for each byte of the id, plus the trailing \0 + static constexpr size_t max_size_of_each = 3; // 2 for each byte, plus the trailing \0 for (size_t i = 0; i < sizeof(id.id); ++i) { - sprintf(idstr + 2 * i, "%02x", id.id[i]); + snprintf(idstr + 2 * i, max_size_of_each, "%02x", id.id[i]); } - idstr[32] = 0; + idstr[sizeof(id.id) * 2] = '\0'; // TODO(yadunund): Can we get the size of the shm from the config even though it's not // a standard parameter? context->impl->shm_manager = diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 19871ac2..e11d2278 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -719,11 +719,10 @@ rmw_publish( return RMW_RET_ERROR; } } - msg_bytes = (char *)zc_shmbuf_ptr(&shmbuf); + msg_bytes = reinterpret_cast(zc_shmbuf_ptr(&shmbuf)); from_shm = true; - } - // Get memory from the allocator. - else { + } else { + // Get memory from the allocator. msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); From b4fb97910e85800b8f0ae2a43dfc83d665625d57 Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 10 Nov 2023 03:46:09 +0800 Subject: [PATCH 50/50] Mock liveliness tokens to update graph cache (#36) * Liveliness for nodes * Close zenoh session in rmw_context_fini * Use PublishToken api * Implement get_node_names * Rely on get to update graph_cache during init Signed-off-by: Yadunund Co-authored-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 2 + rmw_zenoh_cpp/package.xml | 1 + rmw_zenoh_cpp/src/detail/graph_cache.cpp | 339 ++++++++++++++++++++ rmw_zenoh_cpp/src/detail/graph_cache.hpp | 82 +++++ rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 2 - rmw_zenoh_cpp/src/rmw_init.cpp | 111 +++++-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 97 +++--- 7 files changed, 550 insertions(+), 84 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index a2b4bc5e..f4101cfe 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(rcutils REQUIRED) find_package(rosidl_typesupport_fastrtps_c REQUIRED) find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) find_package(rmw REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) find_package(zenoh_c_vendor REQUIRED) find_package(zenohc REQUIRED) @@ -55,6 +56,7 @@ target_link_libraries(rmw_zenoh_cpp rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp rmw::rmw + yaml-cpp zenohc::lib ) diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index a53ba6cc..e95c138b 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -18,6 +18,7 @@ rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp rmw + yaml_cpp_vendor ament_lint_auto ament_lint_common diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 18280c50..a890f485 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -13,12 +13,127 @@ // limitations under the License. #include +#include +#include +#include #include +#include +#include "rcpputils/scope_exit.hpp" + +#include "rcutils/logging_macros.h" #include "rcutils/strdup.h" +#include "rmw/sanity_checks.h" +#include "rmw/error_handling.h" + #include "graph_cache.hpp" +///============================================================================= +std::string GenerateToken::liveliness(size_t domain_id) +{ + std::string token = "@ros2_lv/" + std::to_string(domain_id) + "/**"; + return token; +} + +///============================================================================= +static std::string generate_base_token( + const std::string & entity, + size_t domain_id, + const std::string & namespace_, + const std::string & name) +{ + std::stringstream token_ss; + // TODO(Yadunund): Empty namespace will contain /. Fix non-empty namespace. + token_ss << "@ros2_lv/" << domain_id << "/" << entity << namespace_ << name; + return token_ss.str(); +} + +///============================================================================= +std::string GenerateToken::node( + size_t domain_id, + const std::string & namespace_, + const std::string & name) +{ + return generate_base_token("NN", domain_id, namespace_, name); +} + +///============================================================================= +std::string GenerateToken::publisher( + size_t domain_id, + const std::string & node_namespace, + const std::string & node_name, + const std::string & topic, + const std::string & type, + const std::string & qos) +{ + std::string token = generate_base_token("MP", domain_id, node_namespace, node_name); + token += topic + "/" + type + "/" + qos; + return token; +} + +///============================================================================= +bool PublishToken::put( + z_owned_session_t * session, + const std::string & token) +{ + if (!z_session_check(session)) { + RCUTILS_SET_ERROR_MSG("The zenoh session is invalid."); + return false; + } + + // TODO(Yadunund): z_keyexpr_new creates a copy so find a way to avoid it. + z_owned_keyexpr_t keyexpr = z_keyexpr_new(token.c_str()); + auto drop_keyexpr = rcpputils::make_scope_exit( + [&keyexpr]() { + z_drop(z_move(keyexpr)); + }); + if (!z_keyexpr_check(&keyexpr)) { + RCUTILS_SET_ERROR_MSG("invalid keyexpression generation for liveliness publication."); + return false; + } + RCUTILS_LOG_WARN_NAMED("rmw_zenoh_cpp", "Sending PUT on %s", token.c_str()); + z_put_options_t options = z_put_options_default(); + options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); + if (z_put(z_loan(*session), z_keyexpr(token.c_str()), nullptr, 0, &options) < 0) { + RCUTILS_SET_ERROR_MSG("unable to publish liveliness for node creation"); + return false; + } + + return true; +} + +///============================================================================= +bool PublishToken::del( + z_owned_session_t * session, + const std::string & token) +{ + if (!z_session_check(session)) { + RCUTILS_SET_ERROR_MSG("The zenoh session is invalid."); + return false; + } + + // TODO(Yadunund): z_keyexpr_new creates a copy so find a way to avoid it. + z_owned_keyexpr_t keyexpr = z_keyexpr_new(token.c_str()); + auto drop_keyexpr = rcpputils::make_scope_exit( + [&keyexpr]() { + z_drop(z_move(keyexpr)); + }); + if (!z_keyexpr_check(&keyexpr)) { + RCUTILS_SET_ERROR_MSG("invalid key-expression generation for liveliness publication."); + return false; + } + RCUTILS_LOG_WARN_NAMED("rmw_zenoh_cpp", "Sending DELETE on %s", token.c_str()); + const z_delete_options_t options = z_delete_options_default(); + if (z_delete(z_loan(*session), z_loan(keyexpr), &options) < 0) { + RCUTILS_SET_ERROR_MSG("failed to delete liveliness key"); + return false; + } + + return true; +} + +///============================================================================= PublisherData::PublisherData( const char * topic, const char * node, const char * namespace_, const char * type, rcutils_allocator_t * allocator) @@ -37,6 +152,7 @@ PublisherData::PublisherData( type_name_ = rcutils_strdup(type, *allocator); } +///============================================================================= PublisherData::~PublisherData() { allocator_->deallocate(topic_name_, allocator_->state); @@ -45,6 +161,7 @@ PublisherData::~PublisherData() allocator_->deallocate(type_name_, allocator_->state); } +///============================================================================= SubscriptionData::SubscriptionData( const char * topic, const char * node, const char * namespace_, const char * type, rcutils_allocator_t * allocator) @@ -63,6 +180,7 @@ SubscriptionData::SubscriptionData( type_name_ = rcutils_strdup(type, *allocator); } +///============================================================================= SubscriptionData::~SubscriptionData() { allocator_->deallocate(topic_name_, allocator_->state); @@ -71,6 +189,7 @@ SubscriptionData::~SubscriptionData() allocator_->deallocate(type_name_, allocator_->state); } +///============================================================================= uint64_t GraphCache::add_publisher( const char * topic, const char * node, const char * namespace_, @@ -84,6 +203,7 @@ GraphCache::add_publisher( return this_handle_id; } +///============================================================================= void GraphCache::remove_publisher(uint64_t handle) { @@ -95,6 +215,7 @@ GraphCache::remove_publisher(uint64_t handle) publishers_.erase(handle); } +///============================================================================= uint64_t GraphCache::add_subscription( const char * topic, const char * node, const char * namespace_, @@ -109,6 +230,7 @@ GraphCache::add_subscription( return this_handle_id; } +///============================================================================= void GraphCache::remove_subscription(uint64_t handle) { @@ -119,3 +241,220 @@ GraphCache::remove_subscription(uint64_t handle) subscriptions_.erase(handle); } + +///============================================================================= +static std::vector split_keyexpr(const std::string & keyexpr) +{ + std::vector delim_idx = {}; + // Insert -1 for starting position to make the split easier when using substr. + delim_idx.push_back(-1); + std::size_t idx = 0; + for (auto it = keyexpr.begin(); it != keyexpr.end(); ++it) { + if (*it == '/') { + delim_idx.push_back(idx); + } + ++idx; + } + std::vector result = {}; + try { + for (std::size_t i = 1; i < delim_idx.size(); ++i) { + const auto & prev_idx = delim_idx.at(i - 1); + const auto & idx = delim_idx.at(i); + result.push_back(keyexpr.substr(prev_idx + 1, idx - prev_idx - 1)); + } + } catch (const std::exception & e) { + printf("%s\n", e.what()); + return {}; + } + // Finally add the last substr. + result.push_back(keyexpr.substr(delim_idx.back() + 1)); + return result; +} + +///============================================================================= +void GraphCache::parse_put(const std::string & keyexpr) +{ + // TODO(Yadunund): Validate data. + std::vector parts = split_keyexpr(keyexpr); + if (parts.size() < 3) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Received invalid liveliness token"); + return; + } + // Get the entity, ie N, MP, MS, SS, SC. + const std::string & entity = parts[2]; + std::lock_guard lock(graph_mutex_); + if (entity == "NN") { + // Node + RCUTILS_LOG_WARN_NAMED("rmw_zenoh_cpp", "Adding node %s to the graph.", parts.back().c_str()); + const bool has_namespace = entity.size() == 5 ? true : false; + graph_[parts.back()] = YAML::Node(); + // TODO(Yadunund): Implement enclave support. + graph_[parts.back()]["enclave"] = ""; + graph_[parts.back()]["namespace"] = has_namespace ? parts.at(4) : "/"; + } else if (entity == "MP") { + // Publisher + } else if (entity == "MS") { + // Subscription + } else if (entity == "SS") { + // Service + } else if (entity == "SC") { + // Client + } else { + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Received liveliness token with invalid entity type."); + return; + } +} + +///============================================================================= +void GraphCache::parse_del(const std::string & keyexpr) +{ + // TODO(Yadunund): Validate data. + std::vector parts = split_keyexpr(keyexpr); + if (parts.size() < 3) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Received invalid liveliness token"); + return; + } + // Get the entity, ie N, MP, MS, SS, SC. + const std::string & entity = parts[2]; + std::lock_guard lock(graph_mutex_); + if (entity == "NN") { + // Node + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "Removing node %s from the graph.", + parts.back().c_str() + ); + graph_.remove(entity.back()); + } else if (entity == "MP") { + // Publisher + } else if (entity == "MS") { + // Subscription + } else if (entity == "SS") { + // Service + } else if (entity == "SC") { + // Client + } else { + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Received liveliness token with invalid entity type."); + return; + } +} + +///============================================================================= +rmw_ret_t GraphCache::get_node_names( + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * enclaves, + rcutils_allocator_t * allocator) const +{ + std::lock_guard lock(graph_mutex_); + if (RMW_RET_OK != rmw_check_zero_rmw_string_array(node_names)) { + return RMW_RET_INVALID_ARGUMENT; + } + if (RMW_RET_OK != rmw_check_zero_rmw_string_array(node_namespaces)) { + return RMW_RET_INVALID_ARGUMENT; + } + if ( + enclaves && + RMW_RET_OK != rmw_check_zero_rmw_string_array(enclaves)) + { + return RMW_RET_INVALID_ARGUMENT; + } + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "get_node_names allocator is not valid", return RMW_RET_INVALID_ARGUMENT); + + size_t nodes_number = graph_.size(); + + rcutils_ret_t rcutils_ret = + rcutils_string_array_init(node_names, nodes_number, allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + return RMW_RET_BAD_ALLOC; + } + auto free_node_names = rcpputils::make_scope_exit( + [node_names]() { + rcutils_ret_t ret = rcutils_string_array_fini(node_names); + if (ret != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string().str); + } + }); + + rcutils_ret = + rcutils_string_array_init(node_namespaces, nodes_number, allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + return RMW_RET_BAD_ALLOC; + } + auto free_node_namespaces = rcpputils::make_scope_exit( + [node_namespaces]() { + rcutils_ret_t ret = rcutils_string_array_fini(node_namespaces); + if (ret != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string().str); + } + }); + + auto free_enclaves_lambda = [enclaves]() -> void { + rcutils_ret_t ret = rcutils_string_array_fini(enclaves); + if (ret != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string().str); + } + }; + + std::shared_ptr> free_enclaves{nullptr}; + if (enclaves) { + rcutils_ret = + rcutils_string_array_init(enclaves, nodes_number, allocator); + if (RCUTILS_RET_OK != rcutils_ret) { + return RMW_RET_BAD_ALLOC; + } + free_enclaves = + std::make_shared>( + std::move(free_enclaves_lambda)); + } + + // TODO(Yadunund): Remove this printout. + const std::string & graph_str = YAML::Dump(graph_); + RCUTILS_LOG_WARN_NAMED("rmw_zenoh_cpp", "[graph]\n%s\n", graph_str.c_str()); + // Fill node names, namespaces and enclaves. + std::size_t j = 0; + for (auto it = graph_.begin(); it != graph_.end(); ++it) { + const auto & node_name = it->first.as(); + const auto & yaml_node = it->second; + node_names->data[j] = rcutils_strdup(node_name.c_str(), *allocator); + if (!node_names->data[j]) { + return RMW_RET_BAD_ALLOC; + } + node_namespaces->data[j] = rcutils_strdup( + yaml_node["namespace"].as().c_str(), *allocator); + if (!node_namespaces->data[j]) { + return RMW_RET_BAD_ALLOC; + } + if (enclaves) { + enclaves->data[j] = rcutils_strdup( + yaml_node["enclaves"].as().c_str(), *allocator); + if (!enclaves->data[j]) { + return RMW_RET_BAD_ALLOC; + } + } + ++j; + } + + if (free_enclaves) { + free_enclaves->cancel(); + } + free_node_namespaces.cancel(); + free_node_names.cancel(); + + return RMW_RET_OK; +} diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 6e852be0..0bb878b0 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -15,12 +15,58 @@ #ifndef DETAIL__GRAPH_CACHE_HPP_ #define DETAIL__GRAPH_CACHE_HPP_ +#include + #include #include #include +#include +#include #include "rcutils/allocator.h" +#include "rcutils/types.h" + +#include "rmw/rmw.h" + +#include "yaml-cpp/yaml.h" + +///============================================================================= +class GenerateToken +{ +public: + static std::string liveliness(size_t domain_id); + + /// Returns a string with key-expression @ros2_lv/domain_id/N/namespace/name + static std::string node( + size_t domain_id, + const std::string & namespace_, + const std::string & name); + + static std::string publisher( + size_t domain_id, + const std::string & node_namespace, + const std::string & node_name, + const std::string & topic, + const std::string & type, + const std::string & qos); +}; +///============================================================================= +/// Helper utilities to put/delete tokens until liveliness is supported in the +/// zenoh-c bindings. +class PublishToken +{ +public: + static bool put( + z_owned_session_t * session, + const std::string & token); + + static bool del( + z_owned_session_t * session, + const std::string & token); +}; + +///============================================================================= class PublisherData final { public: @@ -38,6 +84,7 @@ class PublisherData final char * type_name_{nullptr}; }; +///============================================================================= class SubscriptionData final { public: @@ -55,6 +102,7 @@ class SubscriptionData final char * type_name_{nullptr}; }; +///============================================================================= class GraphCache final { public: @@ -72,6 +120,17 @@ class GraphCache final void remove_subscription(uint64_t subscription_handle); + // Parse a PUT message over a token's key-expression and update the graph. + void parse_put(const std::string & keyexpr); + // Parse a DELETE message over a token's key-expression and update the graph. + void parse_del(const std::string & keyexpr); + + rmw_ret_t get_node_names( + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * enclaves, + rcutils_allocator_t * allocator) const; + private: std::mutex publishers_mutex_; uint64_t publishers_handle_id_{0}; @@ -80,6 +139,29 @@ class GraphCache final std::mutex subscriptions_mutex_; uint64_t subscriptions_handle_id_{0}; std::map> subscriptions_; + + /* + node_1: + enclave: + namespace: + publishers: [ + { + topic: + type: + qos: + } + ] + subscriptions: [ + { + topic: + type: + qos: + } + ] + node_n: + */ + YAML::Node graph_; + mutable std::mutex graph_mutex_; }; #endif // DETAIL__GRAPH_CACHE_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index cc43e495..421932ac 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -43,8 +43,6 @@ struct rmw_context_impl_s // The SHM manager. zc_owned_shm_manager_t shm_manager; - z_owned_publisher_t graph_publisher; - z_owned_subscriber_t graph_subscriber; /// Shutdown flag. diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 08e98792..35d8fc1f 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -22,6 +22,7 @@ #include "detail/zenoh_config.hpp" #include "rcutils/env.h" +#include "rcutils/logging_macros.h" #include "rcutils/strdup.h" #include "rcutils/types.h" @@ -43,14 +44,36 @@ static void graph_sub_data_handler( { (void)data; z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "[graph_sub_data_handler] Received key '%s'", + z_loan(keystr) + ); + + // Get the context impl from data. + rmw_context_impl_s * context_impl = static_cast( + data); + if (context_impl == nullptr) { + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "[graph_sub_data_handler] Unable to convert data into context_impl" + ); + return; + } - // TODO(clalancette): Use the allocator here - char * payload_data = static_cast(malloc(sample->payload.len + 1)); - - memcpy(payload_data, sample->payload.start, sample->payload.len); - payload_data[sample->payload.len] = '\0'; - - free(payload_data); + // TODO(Yadunund): Avoid this copy. + std::string keyexpr_str(keystr._cstr); + + switch (sample->kind) { + case z_sample_kind_t::Z_SAMPLE_KIND_PUT: + context_impl->graph_cache.parse_put(keyexpr_str); + break; + case z_sample_kind_t::Z_SAMPLE_KIND_DELETE: + context_impl->graph_cache.parse_del(keyexpr_str); + break; + default: + break; + } z_drop(z_move(keystr)); } @@ -215,12 +238,51 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(gc_data->~GuardCondition(), GuardCondition); }); + // Setup liveliness subscriptions for discovery. + const std::string liveliness_str = GenerateToken::liveliness(context->actual_domain_id); + + // Query the router to get graph information before this session was started. + // TODO(Yadunund): This will not be needed once the zenoh-c liveliness API is available. + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "Sending Query '%s' to fetch discovery data from router...", + liveliness_str.c_str() + ); + z_owned_reply_channel_t channel = zc_reply_fifo_new(16); + z_get_options_t opts = z_get_options_default(); + z_get( + z_loan(context->impl->session), z_keyexpr(liveliness_str.c_str()), "", z_move(channel.send), + &opts); // here, the send is moved and will be dropped by zenoh when adequate + z_owned_reply_t reply = z_reply_null(); + for (z_call(channel.recv, &reply); z_check(reply); z_call(channel.recv, &reply)) { + if (z_reply_is_ok(&reply)) { + z_sample_t sample = z_reply_ok(&reply); + z_owned_str_t keystr = z_keyexpr_to_string(sample.keyexpr); + printf( + ">> [discovery] Received ('%s': '%.*s')\n", z_loan(keystr), + static_cast(sample.payload.len), sample.payload.start); + context->impl->graph_cache.parse_put(z_loan(keystr)); + z_drop(z_move(keystr)); + } else { + printf("[discovery] Received an error\n"); + } + } + z_drop(z_move(reply)); + z_drop(z_move(channel)); + + // TODO(Yadunund): Switch this to a liveliness subscriptions once the API is available. + RCUTILS_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "Setting up liveliness subscription on key: %s", + liveliness_str.c_str() + ); + auto sub_options = z_subscriber_options_default(); sub_options.reliability = Z_RELIABILITY_RELIABLE; z_owned_closure_sample_t callback = z_closure(graph_sub_data_handler, nullptr, context->impl); context->impl->graph_subscriber = z_declare_subscriber( z_loan(context->impl->session), - z_keyexpr("ros/graph/**"), + z_keyexpr(liveliness_str.c_str()), z_move(callback), &sub_options); auto undeclare_z_sub = rcpputils::make_scope_exit( @@ -232,20 +294,6 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_ERROR; } - context->impl->graph_publisher = z_declare_publisher( - z_loan(context->impl->session), - z_keyexpr("ros/graph/hello"), - NULL); - auto undeclare_z_publisher = rcpputils::make_scope_exit( - [context]() { - z_undeclare_publisher(z_move(context->impl->graph_publisher)); - }); - if (!z_check(context->impl->graph_subscriber)) { - RMW_SET_ERROR_MSG("unable to create zenoh subscription"); - return RMW_RET_ERROR; - } - - undeclare_z_publisher.cancel(); undeclare_z_sub.cancel(); close_session.cancel(); destruct_guard_condition_data.cancel(); @@ -277,14 +325,6 @@ rmw_shutdown(rmw_context_t * context) rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - z_undeclare_publisher(z_move(context->impl->graph_publisher)); - z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); - - // Close the zenoh session - if (z_close(z_move(context->impl->session)) < 0) { - RMW_SET_ERROR_MSG("Error while closing zenoh session"); - return RMW_RET_ERROR; - } context->impl->is_shutdown = true; return RMW_RET_OK; } @@ -309,6 +349,17 @@ rmw_context_fini(rmw_context_t * context) return RMW_RET_INVALID_ARGUMENT; } + // We destroy zenoh artifacts here instead of rmw_shutdown() as + // rmw_shutdown() is invoked before rmw_destroy_node() however we still need the session + // alive for the latter. + // TODO(Yadunund): Check if this is a bug in rmw. + z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); + // Close the zenoh session + if (z_close(z_move(context->impl->session)) < 0) { + RMW_SET_ERROR_MSG("Error while closing zenoh session"); + return RMW_RET_ERROR; + } + const rcutils_allocator_t * allocator = &context->options.allocator; z_drop(z_move(context->impl->shm_manager)); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index e11d2278..5df96482 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -21,6 +21,7 @@ #include #include "detail/guard_condition.hpp" +#include "detail/graph_cache.hpp" #include "detail/identifier.hpp" #include "detail/message_type_support.hpp" #include "detail/rmw_data_types.hpp" @@ -180,19 +181,18 @@ rmw_create_node( node->context = context; // Publish to the graph that a new node is in town - z_publisher_put_options_t pub_options = z_publisher_put_options_default(); - pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); - z_publisher_put( - z_loan(context->impl->graph_publisher), - reinterpret_cast(node->name), - name_len, - &pub_options); + const bool result = PublishToken::put( + &node->context->impl->session, + GenerateToken::node(context->actual_domain_id, namespace_, name) + ); + if (!result) { + return nullptr; + } free_node_data.cancel(); free_namespace.cancel(); free_name.cancel(); free_node.cancel(); - return node; } @@ -202,6 +202,8 @@ rmw_ret_t rmw_destroy_node(rmw_node_t * node) { RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, @@ -209,13 +211,13 @@ rmw_destroy_node(rmw_node_t * node) return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); // Publish to the graph that a node has ridden off into the sunset - z_publisher_put_options_t pub_options = z_publisher_put_options_default(); - pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); - z_publisher_put( - z_loan(node->context->impl->graph_publisher), - reinterpret_cast(node->name), - strlen(node->name), - &pub_options); + const bool result = PublishToken::del( + &node->context->impl->session, + GenerateToken::node(node->context->actual_domain_id, node->namespace_, node->name) + ); + if (!result) { + return RMW_RET_ERROR; + } rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -515,13 +517,7 @@ rmw_create_publisher( }); // Publish to the graph that a new publisher is in town - z_publisher_put_options_t pub_options = z_publisher_put_options_default(); - pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); - z_publisher_put( - z_loan(node->context->impl->graph_publisher), - reinterpret_cast(rmw_publisher->topic_name), - topic_len, - &pub_options); + // TODO(Yadunund): Publish liveliness for the new publisher. publisher_data->graph_cache_handle = node->context->impl->graph_cache.add_publisher( rmw_publisher->topic_name, node->name, node->namespace_, @@ -563,13 +559,7 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) rmw_ret_t ret = RMW_RET_OK; // Publish to the graph that a publisher has ridden off into the sunset - z_publisher_put_options_t pub_options = z_publisher_put_options_default(); - pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); - z_publisher_put( - z_loan(node->context->impl->graph_publisher), - reinterpret_cast(publisher->topic_name), - strlen(publisher->topic_name), - &pub_options); + // TODO(Yadunund): Publish liveliness for the deleted publisher. rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -1184,13 +1174,8 @@ rmw_create_subscription( }); // Publish to the graph that a new subscription is in town - z_publisher_put_options_t pub_options = z_publisher_put_options_default(); - pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); - z_publisher_put( - z_loan(node->context->impl->graph_publisher), - reinterpret_cast(rmw_subscription->topic_name), - topic_len, - &pub_options); + // TODO(Yadunund): Publish liveliness for the new subscription. + sub_data->graph_cache_handle = node->context->impl->graph_cache.add_subscription( rmw_subscription->topic_name, node->name, node->namespace_, @@ -1233,13 +1218,7 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) rmw_ret_t ret = RMW_RET_OK; // Publish to the graph that a subscription has ridden off into the sunset - z_publisher_put_options_t pub_options = z_publisher_put_options_default(); - pub_options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); - z_publisher_put( - z_loan(node->context->impl->graph_publisher), - reinterpret_cast(subscription->topic_name), - strlen(subscription->topic_name), - &pub_options); + // TODO(Yadunund): Publish liveliness for the deleted subscription. rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -2020,10 +1999,17 @@ rmw_get_node_names( rcutils_string_array_t * node_names, rcutils_string_array_t * node_namespaces) { - static_cast(node); - static_cast(node_names); - static_cast(node_namespaces); - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node_names, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node_namespaces, RMW_RET_INVALID_ARGUMENT); + + rcutils_allocator_t * allocator = &node->context->options.allocator; + RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); + + return node->context->impl->graph_cache.get_node_names( + node_names, node_namespaces, nullptr, allocator); } //============================================================================== @@ -2035,11 +2021,18 @@ rmw_get_node_names_with_enclaves( rcutils_string_array_t * node_namespaces, rcutils_string_array_t * enclaves) { - static_cast(node); - static_cast(node_names); - static_cast(node_namespaces); - static_cast(enclaves); - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node_names, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node_namespaces, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(enclaves, RMW_RET_INVALID_ARGUMENT); + + rcutils_allocator_t * allocator = &node->context->options.allocator; + RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); + + return node->context->impl->graph_cache.get_node_names( + node_names, node_namespaces, enclaves, allocator); } //==============================================================================