From 8d0e3c81df7561eca34caa62bdb30af978cbb773 Mon Sep 17 00:00:00 2001 From: Luca Cominardi Date: Tue, 23 Jul 2024 09:48:03 +0200 Subject: [PATCH 01/41] Update style.yaml Signed-off-by: Luca Cominardi --- .github/workflows/style.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index d20c770a..2a4d6531 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -8,7 +8,7 @@ defaults: shell: bash jobs: test: - runs-on: ubuntu-latest + runs-on: [self-hosted, ubuntu-24.04] strategy: fail-fast: false matrix: From 62433e7f9b29bdf03e42a513df3d043355824de0 Mon Sep 17 00:00:00 2001 From: Luca Cominardi Date: Tue, 23 Jul 2024 09:48:13 +0200 Subject: [PATCH 02/41] Update check_logging.yaml Signed-off-by: Luca Cominardi --- .github/workflows/check_logging.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/check_logging.yaml b/.github/workflows/check_logging.yaml index caffa805..9e543644 100644 --- a/.github/workflows/check_logging.yaml +++ b/.github/workflows/check_logging.yaml @@ -4,7 +4,7 @@ name: "Check logging macros" on: [pull_request] jobs: check_logging: - runs-on: ubuntu-latest + runs-on: [self-hosted, ubuntu-24.04] steps: - name: Check logging macros uses: JJ/github-pr-contains-action@releases/v14.1 From 4535aea00b20da3d5c06745aee31b731c1036532 Mon Sep 17 00:00:00 2001 From: Luca Cominardi Date: Tue, 23 Jul 2024 09:48:29 +0200 Subject: [PATCH 03/41] Update build.yaml Signed-off-by: Luca Cominardi --- .github/workflows/build.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index eff5238d..b9029938 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -9,7 +9,7 @@ defaults: shell: bash jobs: test: - runs-on: ubuntu-latest + runs-on: [self-hosted, ubuntu-24.04] strategy: fail-fast: false matrix: From 109864b86928a00b99ec10390448b3d3d71f615e Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 20 Jun 2024 14:44:40 +0800 Subject: [PATCH 04/41] chore: configure the compiliation chore: checkout dev/1.0.0 --- zenoh_c_vendor/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 7903e1e8..943ebb1d 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ament_cmake_vendor_package REQUIRED) # Note: We separate the two args needed for cargo with "$" and not ";" as the # latter is a list separater in cmake and hence the string will be split into two # when expanded. -set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/shared-memory zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") +set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memory zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") # Set VCS_VERSION to include latest changes from zenoh-c to benefit from : # - https://github.com/eclipse-zenoh/zenoh-c/pull/340 (fix build issue) @@ -24,8 +24,8 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/shared # - https://github.com/eclipse-zenoh/zenoh-c/pull/358 (fix debian packaging issue: https://github.com/jspricke/ros-deb-builder-action/issues/49) # - https://github.com/eclipse-zenoh/zenoh/pull/1150 (fix deadlock issue https://github.com/ros2/rmw_zenoh/issues/182) ament_vendor(zenoh_c_vendor - VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git - VCS_VERSION 548ee8dde0f53a58c06e68a2949949b31140c36c + VCS_URL https://github.com/eclipse-zenoh/zenoh-c + VCS_VERSION dev/1.0.0 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" ) From c7724ff7cb2e0da21eee65c0f211b43bf87b0f8e Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 25 Jun 2024 18:09:27 +0800 Subject: [PATCH 05/41] chore: complete the 1st version --- .clang_format | 20 + .gitignore | 4 + .../src/detail/attachment_helpers.cpp | 149 +- .../src/detail/attachment_helpers.hpp | 43 +- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 1 - rmw_zenoh_cpp/src/detail/graph_cache.hpp | 5 +- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 1 - rmw_zenoh_cpp/src/detail/liveliness_utils.hpp | 1 - rmw_zenoh_cpp/src/detail/logging.cpp | 1 + rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 112 +- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 23 +- rmw_zenoh_cpp/src/detail/type_support.cpp | 2 - rmw_zenoh_cpp/src/detail/type_support.hpp | 1 - rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 6 +- rmw_zenoh_cpp/src/detail/zenoh_config.hpp | 3 - .../src/detail/zenoh_router_check.cpp | 7 +- .../src/detail/zenoh_router_check.hpp | 2 +- rmw_zenoh_cpp/src/rmw_event.cpp | 2 + rmw_zenoh_cpp/src/rmw_init.cpp | 487 ++- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 3758 ++++++++--------- rmw_zenoh_cpp/src/zenohd/main.cpp | 4 +- 21 files changed, 2280 insertions(+), 2352 deletions(-) create mode 100644 .clang_format create mode 100644 .gitignore diff --git a/.clang_format b/.clang_format new file mode 100644 index 00000000..4a79087d --- /dev/null +++ b/.clang_format @@ -0,0 +1,20 @@ +--- +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterEnum: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false +... diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..01716761 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +build +install +.cache +log diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 46191ecc..ce94663b 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -12,85 +12,152 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include +#include #include "rmw/types.h" +#include "logging_macros.hpp" + #include "attachment_helpers.hpp" -namespace rmw_zenoh_cpp -{ -bool get_gid_from_attachment( - const z_attachment_t * const attachment, uint8_t gid[RMW_GID_STORAGE_SIZE]) -{ - if (!z_check(*attachment)) { +namespace rmw_zenoh_cpp { + +bool create_attachment_iter(z_owned_bytes_t *kv_pair, void *context) { + attachement_context_t *ctx = (attachement_context_t *)context; + z_owned_bytes_t k, v; + + if (ctx->idx == 0) { + z_bytes_serialize_from_str(&k, "sequence_number"); + z_bytes_serialize_from_int64(&v, ctx->data->sequence_number); + } else if (ctx->idx == 1) { + z_bytes_serialize_from_str(&k, "source_timestamp"); + z_bytes_serialize_from_int64(&v, ctx->data->source_timestamp); + } else if (ctx->idx == 2) { + z_bytes_serialize_from_str(&k, "source_gid"); + z_bytes_serialize_from_slice_copy(&v, ctx->data->source_gid, + RMW_GID_STORAGE_SIZE); + } else { return false; } - z_bytes_t index = z_attachment_get(*attachment, z_bytes_new("source_gid")); - if (!z_check(index)) { + z_bytes_serialize_from_pair(kv_pair, z_move(k), z_move(v)); + ctx->idx += 1; + return true; +} + +z_error_t attachement_data_t::serialize_to_zbytes(z_owned_bytes_t *attachment) { + attachement_context_t context = attachement_context_t(this); + return z_bytes_serialize_from_iter(attachment, create_attachment_iter, + (void *)&context); +} + +bool get_attachment(const z_loaned_bytes_t *const attachment, + const std::string &key, z_owned_bytes_t *val) { + if (z_bytes_is_empty(attachment)) { return false; } - if (index.len != RMW_GID_STORAGE_SIZE) { + z_bytes_iterator_t iter = z_bytes_get_iterator(attachment); + z_owned_bytes_t pair, key_; + bool found = false; + + while (z_bytes_iterator_next(&iter, &pair)) { + z_bytes_deserialize_into_pair(z_loan(pair), &key_, val); + z_owned_string_t key_string; + z_bytes_deserialize_into_string(z_loan(key_), &key_string); + + char dbg_info[120]; + sprintf(dbg_info, "Given key: %s, found: %s", key.c_str(), + z_string_data(z_loan(key_string))); + sprintf(dbg_info, "Given key: %s, found: %.*s", key.c_str(), + (int)z_string_len(z_loan(key_string)), + z_string_data(z_loan(key_string))); + + std::string found_key; + found_key.assign(z_string_data(z_loan(key_string)), z_string_len(z_loan(key_string))); + if (found_key == key) { + // if (strcmp(z_string_data(z_loan(key_string)), key.c_str()) == 0) { + found = true; + } + + z_drop(z_move(pair)); + z_drop(z_move(key_)); + z_drop(z_move(key_string)); + + if (found) { + break; + } + } + + if (!found) { return false; } - memcpy(gid, index.start, index.len); + if (z_bytes_is_empty(z_loan(*val))) { + return false; + } return true; } -int64_t get_int64_from_attachment( - const z_attachment_t * const attachment, const std::string & name) -{ - if (!z_check(*attachment)) { - // A valid request must have had an attachment - return -1; +bool get_gid_from_attachment(const z_loaned_bytes_t *const attachment, + uint8_t gid[RMW_GID_STORAGE_SIZE]) { + + if (z_bytes_is_empty(attachment)) { + return false; } - z_bytes_t index = z_attachment_get(*attachment, z_bytes_new(name.c_str())); - if (!z_check(index)) { - return -1; + z_owned_bytes_t val; + if (!get_attachment(attachment, "source_gid", &val)) { + return false; } - if (index.len < 1) { - return -1; + z_owned_slice_t slice; + z_bytes_deserialize_into_slice(z_loan(val), &slice); + if (z_slice_len(z_loan(slice)) != RMW_GID_STORAGE_SIZE) { + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "GID length mismatched.") + return false; } + memcpy(gid, z_slice_data(z_loan(slice)), z_slice_len(z_loan(slice))); - if (index.len > 19) { - // The number was larger than we expected + z_drop(z_move(val)); + z_drop(z_move(slice)); + + return true; +} + +int64_t get_int64_from_attachment(const z_loaned_bytes_t *const attachment, + const std::string &name) { + // A valid request must have had an attachment + if (z_bytes_is_empty(attachment)) { return -1; } - // The largest possible int64_t number is INT64_MAX, i.e. 9223372036854775807. - // That is 19 characters long, plus one for the trailing \0, means we need 20 bytes. - char int64_str[20]; + // TODO(yuyuan): This key should be specific + z_owned_bytes_t val; + if (!get_attachment(attachment, name, &val)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Failed to deserialize int64 from the attachment.") + return false; + } - memcpy(int64_str, index.start, index.len); - int64_str[index.len] = '\0'; + int64_t num; + if (z_bytes_deserialize_into_int64(z_loan(val), &num)) { + return -1; + } - errno = 0; - char * endptr; - int64_t num = strtol(int64_str, &endptr, 10); if (num == 0) { // This is an error regardless; the client should never send this return -1; - } else if (endptr == int64_str) { - // No values were converted, this is an error - return -1; - } else if (*endptr != '\0') { - // There was junk after the number - return -1; - } else if (errno != 0) { - // Some other error occurred, which may include overflow or underflow - return -1; } return num; } -} // namespace rmw_zenoh_cpp + +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp index e27dc886..bcc15e92 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -21,13 +21,40 @@ #include "rmw/types.h" -namespace rmw_zenoh_cpp -{ -bool get_gid_from_attachment( - const z_attachment_t * const attachment, uint8_t gid[RMW_GID_STORAGE_SIZE]); +namespace rmw_zenoh_cpp { -int64_t get_int64_from_attachment( - const z_attachment_t * const attachment, const std::string & name); -} // namespace rmw_zenoh_cpp +class attachement_data_t final { +public: + int64_t sequence_number; + int64_t source_timestamp; + uint8_t source_gid[RMW_GID_STORAGE_SIZE]; + attachement_data_t(const int64_t _sequence_number, + const int64_t _source_timestamp, + const uint8_t _source_gid[RMW_GID_STORAGE_SIZE]) { + sequence_number = _sequence_number; + source_timestamp = _source_timestamp; + memcpy(source_gid, _source_gid, RMW_GID_STORAGE_SIZE); + } + z_error_t serialize_to_zbytes(z_owned_bytes_t *); +}; -#endif // DETAIL__ATTACHMENT_HELPERS_HPP_ +class attachement_context_t final { +public: + const attachement_data_t *data; + int length = 3; + int idx = 0; + + attachement_context_t(const attachement_data_t *_data) : data(_data) {} +}; + +bool get_attachment(const z_loaned_bytes_t *const attachment, + const std::string &key, z_owned_bytes_t *val); + +bool get_gid_from_attachment(const z_loaned_bytes_t *const attachment, + uint8_t gid[RMW_GID_STORAGE_SIZE]); + +int64_t get_int64_from_attachment(const z_loaned_bytes_t *const attachment, + const std::string &name); +} // namespace rmw_zenoh_cpp + +#endif // DETAIL__ATTACHMENT_HELPERS_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 79bff430..54bb69dd 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 47d33765..7fde2bab 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -22,17 +22,14 @@ #include #include #include -#include #include "event.hpp" #include "liveliness_utils.hpp" #include "ordered_map.hpp" #include "rcutils/allocator.h" -#include "rcutils/types.h" -#include "rmw/rmw.h" -#include "rmw/get_topic_endpoint_info.h" +#include "rmw/topic_endpoint_info_array.h" #include "rmw/names_and_types.h" diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index acd71e58..26d5d1b6 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index 13a682e7..f967a229 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include "rmw/types.h" diff --git a/rmw_zenoh_cpp/src/detail/logging.cpp b/rmw_zenoh_cpp/src/detail/logging.cpp index 1977b0f2..2021f90f 100644 --- a/rmw_zenoh_cpp/src/detail/logging.cpp +++ b/rmw_zenoh_cpp/src/detail/logging.cpp @@ -63,4 +63,5 @@ void Logger::log_named( ); } } + } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 51f84b33..3406cf4c 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -18,10 +18,10 @@ #include #include #include -#include #include #include #include +#include #include "logging_macros.hpp" @@ -64,7 +64,7 @@ namespace rmw_zenoh_cpp { ///============================================================================= saved_msg_data::saved_msg_data( - zc_owned_payload_t p, + z_owned_bytes_t p, uint64_t recv_ts, const uint8_t pub_gid[RMW_GID_STORAGE_SIZE], int64_t seqnum, @@ -237,6 +237,7 @@ void rmw_service_data_t::notify() } } + ///============================================================================= void rmw_service_data_t::add_new_query(std::unique_ptr query) { @@ -245,14 +246,15 @@ void rmw_service_data_t::add_new_query(std::unique_ptr query) query_queue_.size() >= adapted_qos_profile.depth) { // Log warning if message is discarded due to hitting the queue depth - z_owned_str_t keystr = z_keyexpr_to_string(z_loan(this->keyexpr)); + z_view_string_t keystr; + z_keyexpr_as_view_string(z_loan(this->keyexpr), &keystr); + RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Query queue depth of %ld reached, discarding oldest Query " "for service for %s", adapted_qos_profile.depth, - z_loan(keystr)); - z_drop(z_move(keystr)); + z_string_data(z_loan(keystr))); query_queue_.pop_front(); } query_queue_.emplace_back(std::move(query)); @@ -340,14 +342,14 @@ void rmw_client_data_t::add_new_reply(std::unique_ptr reply) reply_queue_.size() >= adapted_qos_profile.depth) { // Log warning if message is discarded due to hitting the queue depth - z_owned_str_t keystr = z_keyexpr_to_string(z_loan(this->keyexpr)); + z_view_string_t keystr; + z_keyexpr_as_view_string(z_loan(this->keyexpr), &keystr); RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Reply queue depth of %ld reached, discarding oldest reply " "for client for %s", adapted_qos_profile.depth, - z_loan(keystr)); - z_drop(z_move(keystr)); + z_string_data(z_loan(keystr))); reply_queue_.pop_front(); } reply_queue_.emplace_back(std::move(reply)); @@ -432,14 +434,11 @@ bool rmw_client_data_t::is_shutdown() const //============================================================================== void sub_data_handler( - const z_sample_t * sample, + const z_loaned_sample_t * sample, void * data) { - z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); - auto drop_keystr = rcpputils::make_scope_exit( - [&keystr]() { - z_drop(z_move(keystr)); - }); + z_view_string_t keystr; + z_keyexpr_as_view_string(z_sample_keyexpr(sample), &keystr); auto sub_data = static_cast(data); if (sub_data == nullptr) { @@ -447,13 +446,14 @@ void sub_data_handler( "rmw_zenoh_cpp", "Unable to obtain rmw_subscription_data_t from data for " "subscription for %s", - z_loan(keystr) + z_string_data(z_loan(keystr)) ); return; } uint8_t pub_gid[RMW_GID_STORAGE_SIZE]; - if (!get_gid_from_attachment(&sample->attachment, pub_gid)) { + const z_loaned_bytes_t* attachment = z_sample_attachment(sample); + if (!get_gid_from_attachment(attachment, pub_gid)) { // We failed to get the GID from the attachment. While this isn't fatal, // it is unusual and so we should report it. memset(pub_gid, 0, RMW_GID_STORAGE_SIZE); @@ -462,7 +462,7 @@ void sub_data_handler( "Unable to obtain publisher GID from the attachment."); } - int64_t sequence_number = get_int64_from_attachment(&sample->attachment, "sequence_number"); + int64_t sequence_number = get_int64_from_attachment(attachment, "sequence_number"); if (sequence_number < 0) { // We failed to get the sequence number from the attachment. While this // isn't fatal, it is unusual and so we should report it. @@ -471,7 +471,7 @@ void sub_data_handler( "rmw_zenoh_cpp", "Unable to obtain sequence number from the attachment."); } - int64_t source_timestamp = get_int64_from_attachment(&sample->attachment, "source_timestamp"); + int64_t source_timestamp = get_int64_from_attachment(attachment, "source_timestamp"); if (source_timestamp < 0) { // We failed to get the source timestamp from the attachment. While this // isn't fatal, it is unusual and so we should report it. @@ -480,38 +480,38 @@ void sub_data_handler( "rmw_zenoh_cpp", "Unable to obtain sequence number from the attachment."); } + z_owned_bytes_t payload; + z_bytes_clone(&payload, z_sample_payload(sample)); sub_data->add_new_message( std::make_unique( - zc_sample_payload_rcinc(sample), - sample->timestamp.time, pub_gid, sequence_number, source_timestamp), z_loan(keystr)); + payload, + z_timestamp_ntp64_time(z_sample_timestamp(sample)), + pub_gid, sequence_number, source_timestamp), z_string_data(z_loan(keystr))); } ///============================================================================= -ZenohQuery::ZenohQuery(const z_query_t * query) +ZenohQuery::ZenohQuery(const z_loaned_query_t * query) { - query_ = z_query_clone(query); + z_query_clone(query_, query); } ///============================================================================= ZenohQuery::~ZenohQuery() { - z_drop(z_move(query_)); + z_drop(z_move(*query_)); } ///============================================================================= -const z_query_t ZenohQuery::get_query() const +const z_loaned_query_t * ZenohQuery::get_query() const { - return z_query_loan(&query_); + return z_query_loan(query_); } //============================================================================== -void service_data_handler(const z_query_t * query, void * data) +void service_data_handler(const z_loaned_query_t * query, void * data) { - z_owned_str_t keystr = z_keyexpr_to_string(z_query_keyexpr(query)); - auto drop_keystr = rcpputils::make_scope_exit( - [&keystr]() { - z_drop(z_move(keystr)); - }); + z_view_string_t keystr; + z_keyexpr_as_view_string(z_query_keyexpr(query), &keystr); rmw_service_data_t * service_data = static_cast(data); @@ -520,7 +520,7 @@ void service_data_handler(const z_query_t * query, void * data) "rmw_zenoh_cpp", "Unable to obtain rmw_service_data_t from data for " "service for %s", - z_loan(keystr) + z_string_data(z_loan(keystr)) ); return; } @@ -540,14 +540,12 @@ ZenohReply::~ZenohReply() z_reply_drop(z_move(reply_)); } +// TODO(yuyuan): z_reply_ok return a null pointer if not z_reply_is_ok, +// so that's remove the additional optional wrapper. ///============================================================================= -std::optional ZenohReply::get_sample() const +const z_loaned_sample_t * ZenohReply::get_sample() const { - if (z_reply_is_ok(&reply_)) { - return z_reply_ok(&reply_); - } - - return std::nullopt; + return z_reply_ok(z_loan(reply_)); } ///============================================================================= @@ -558,7 +556,7 @@ size_t rmw_client_data_t::get_next_sequence_number() } //============================================================================== -void client_data_handler(z_owned_reply_t * reply, void * data) +void client_data_handler(const z_loaned_reply_t * reply, void * data) { auto client_data = static_cast(data); if (client_data == nullptr) { @@ -575,30 +573,36 @@ void client_data_handler(z_owned_reply_t * reply, void * data) return; } - if (!z_reply_check(reply)) { + if (!z_reply_is_ok(reply)) { + z_view_string_t keystr; + z_keyexpr_as_view_string(z_loan(client_data->keyexpr), &keystr); + const z_loaned_reply_err_t* err = z_reply_err(reply); + const z_loaned_bytes_t* err_payload = z_reply_err_payload(err); + + // TODO(yuyuan): z_view_string_t? + z_owned_string_t err_str; + z_bytes_deserialize_into_string(err_payload, &err_str); RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", - "z_reply_check returned False" - ); + "z_reply_is_ok returned False for keyexpr %s. Reason: %.*s", + z_string_data(z_loan(keystr)), + (int)z_string_len(z_loan(err_str)), + z_string_data(z_loan(err_str))); + z_drop(z_move(err_str)); return; } - if (!z_reply_is_ok(reply)) { - z_owned_str_t keystr = z_keyexpr_to_string(z_loan(client_data->keyexpr)); - z_value_t err = z_reply_err(reply); + z_owned_reply_t owned_reply; + z_reply_clone(&owned_reply, reply); + + if (!z_reply_check(&owned_reply)) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", - "z_reply_is_ok returned False for keyexpr %s. Reason: %.*s", - z_loan(keystr), - (int)err.payload.len, - err.payload.start); - z_drop(z_move(keystr)); - + "z_reply_check returned False" + ); return; } - client_data->add_new_reply(std::make_unique(reply)); - // Since we took ownership of the reply, null it out here - *reply = z_reply_null(); + client_data->add_new_reply(std::make_unique(&owned_reply)); } void client_data_drop(void * data) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 60e4bd01..dad46940 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -52,9 +52,10 @@ class rmw_context_impl_s final // An owned session. z_owned_session_t session; + // TODO(yuyuan): SHM provider // An optional SHM manager that is initialized of SHM is enabled in the // zenoh session config. - std::optional shm_manager; + // std::optional shm_manager; z_owned_subscriber_t graph_subscriber; @@ -131,12 +132,12 @@ class rmw_publisher_data_t final ///============================================================================= // z_owned_closure_sample_t -void sub_data_handler(const z_sample_t * sample, void * sub_data); +void sub_data_handler(const z_loaned_sample_t * sample, void * sub_data); struct saved_msg_data { explicit saved_msg_data( - zc_owned_payload_t p, + z_owned_bytes_t p, uint64_t recv_ts, const uint8_t pub_gid[RMW_GID_STORAGE_SIZE], int64_t seqnum, @@ -144,7 +145,7 @@ struct saved_msg_data ~saved_msg_data(); - zc_owned_payload_t payload; + z_owned_bytes_t payload; uint64_t recv_timestamp; uint8_t publisher_gid[RMW_GID_STORAGE_SIZE]; int64_t sequence_number; @@ -200,24 +201,24 @@ class rmw_subscription_data_t final ///============================================================================= -void service_data_handler(const z_query_t * query, void * service_data); +void service_data_handler(const z_loaned_query_t * query, void * service_data); ///============================================================================= -void client_data_handler(z_owned_reply_t * reply, void * client_data); +void client_data_handler(const z_loaned_reply_t * reply, void * client_data); void client_data_drop(void * data); ///============================================================================= class ZenohQuery final { public: - ZenohQuery(const z_query_t * query); + ZenohQuery(const z_loaned_query_t * query); ~ZenohQuery(); - const z_query_t get_query() const; + const z_loaned_query_t * get_query() const; private: - z_owned_query_t query_; + z_owned_query_t * query_; }; ///============================================================================= @@ -284,7 +285,8 @@ class ZenohReply final ~ZenohReply(); - std::optional get_sample() const; + // TODO(yuyuan): rename this function + const z_loaned_sample_t * get_sample() const; private: z_owned_reply_t reply_; @@ -349,6 +351,7 @@ class rmw_client_data_t final rmw_wait_set_data_t * wait_set_data_{nullptr}; std::mutex condition_mutex_; + // TODO(yuyuan): replace with zenoh-c ring buffer handler std::deque> reply_queue_; mutable std::mutex reply_queue_mutex_; diff --git a/rmw_zenoh_cpp/src/detail/type_support.cpp b/rmw_zenoh_cpp/src/detail/type_support.cpp index fff22474..b2cc2848 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/type_support.cpp @@ -17,8 +17,6 @@ // https://github.com/ros2/rmw_fastrtps/blob/469624e3d483290d6f88fe4b89ee5feaa7694e61/rmw_fastrtps_cpp/src/type_support_common.hpp #include -#include -#include #include "rmw/error_handling.h" diff --git a/rmw_zenoh_cpp/src/detail/type_support.hpp b/rmw_zenoh_cpp/src/detail/type_support.hpp index a4714863..c7b2be52 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support.hpp @@ -19,7 +19,6 @@ #ifndef DETAIL__TYPE_SUPPORT_HPP_ #define DETAIL__TYPE_SUPPORT_HPP_ -#include #include #include "fastcdr/Cdr.h" diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 5cb29172..5e205196 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include "logging_macros.hpp" @@ -58,11 +60,11 @@ rmw_ret_t _get_z_config( // If the environment variable contains a path to a file, try to read the configuration from it. if (envar_uri[0] != '\0') { // If the environment variable is set, try to read the configuration from the file. - *config = zc_config_from_file(envar_uri); + zc_config_from_file(config, envar_uri); configured_uri = envar_uri; } else { // If the environment variable is not set use internal configuration - *config = zc_config_from_file(default_uri); + zc_config_from_file(config, default_uri); configured_uri = default_uri; } // Verify that the configuration is valid. diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp index dceafe5d..357c7e89 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -16,10 +16,7 @@ #define DETAIL__ZENOH_CONFIG_HPP_ #include - #include -#include -#include #include "rmw/ret_types.h" diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp index ead5ca5a..7d7f5cd9 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp @@ -16,8 +16,6 @@ #include -#include -#include #include #include "logging_macros.hpp" @@ -26,7 +24,7 @@ namespace rmw_zenoh_cpp { ///============================================================================= -rmw_ret_t zenoh_router_check(z_session_t session) +rmw_ret_t zenoh_router_check(const z_loaned_session_t* session) { // Initialize context for callback int context = 0; @@ -43,7 +41,8 @@ rmw_ret_t zenoh_router_check(z_session_t session) }; rmw_ret_t ret = RMW_RET_OK; - z_owned_closure_zid_t router_callback = z_closure(callback, nullptr /* drop */, &context); + z_owned_closure_zid_t router_callback; + z_closure(&router_callback, callback, NULL, &context); if (z_info_routers_zid(session, z_move(router_callback))) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp index f53bc2f5..b6a56b05 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp @@ -24,7 +24,7 @@ namespace rmw_zenoh_cpp /// Check if a Zenoh router is connected to the session. /// @param session Zenoh session to check. /// @return RMW_RET_OK if a Zenoh router is connected to the session. -rmw_ret_t zenoh_router_check(z_session_t session); +rmw_ret_t zenoh_router_check(const z_loaned_session_t* session); } // namespace rmw_zenoh_cpp #endif // DETAIL__ZENOH_ROUTER_CHECK_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index 964118ee..d9781d9d 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -21,10 +21,12 @@ #include "detail/graph_cache.hpp" #include "detail/identifier.hpp" #include "detail/rmw_data_types.hpp" +#include "detail/logging_macros.hpp" extern "C" { + ///============================================================================= /// Initialize a rmw publisher event rmw_ret_t diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 99c6f3ad..90ede057 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -30,14 +30,14 @@ #include "rcutils/strdup.h" #include "rcutils/types.h" -#include "rmw/init.h" #include "rmw/impl/cpp/macros.hpp" +#include "rmw/init.h" #include "rmw/init_options.h" #include "rcpputils/scope_exit.hpp" -extern "C" -{ + +extern "C" { // Megabytes of SHM to reserve. // TODO(clalancette): Make this configurable, or get it from the configuration #define SHM_BUFFER_SIZE_MB 10 @@ -45,19 +45,16 @@ extern "C" namespace { void -graph_sub_data_handler(const z_sample_t * sample, void * data) +graph_sub_data_handler(const z_loaned_sample_t * sample, void * data) { + static_cast(data); - z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); - auto free_keystr = rcpputils::make_scope_exit( - [&keystr]() { - z_drop(z_move(keystr)); - }); + z_view_string_t keystr; + z_keyexpr_as_view_string(z_sample_keyexpr(sample), &keystr); // Get the context impl from data. - rmw_context_impl_s * context_impl = static_cast( - data); + rmw_context_impl_s *context_impl = static_cast(data); if (context_impl == nullptr) { RMW_ZENOH_LOG_WARN_NAMED( "rmw_zenoh_cpp", @@ -66,18 +63,21 @@ graph_sub_data_handler(const z_sample_t * sample, void * data) return; } - switch (sample->kind) { - case z_sample_kind_t::Z_SAMPLE_KIND_PUT: - context_impl->graph_cache->parse_put(keystr._cstr); - break; - case z_sample_kind_t::Z_SAMPLE_KIND_DELETE: - context_impl->graph_cache->parse_del(keystr._cstr); - break; - default: - return; + + std::string str(z_string_data(z_loan(keystr)), z_string_len(z_loan(keystr))); + switch (z_sample_kind(sample)) { + case z_sample_kind_t::Z_SAMPLE_KIND_PUT: + context_impl->graph_cache->parse_put(str); + break; + case z_sample_kind_t::Z_SAMPLE_KIND_DELETE: + context_impl->graph_cache->parse_del(str); + break; + default: + return; } - rmw_ret_t rmw_ret = rmw_trigger_guard_condition(context_impl->graph_guard_condition); + rmw_ret_t rmw_ret = + rmw_trigger_guard_condition(context_impl->graph_guard_condition); if (RMW_RET_OK != rmw_ret) { RMW_ZENOH_LOG_WARN_NAMED( "rmw_zenoh_cpp", @@ -89,81 +89,70 @@ graph_sub_data_handler(const z_sample_t * sample, void * data) //============================================================================== /// 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_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_cpp::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); + 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_cpp::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]() { *context = rmw_get_zero_initialized_context(); }); context->instance_id = options->instance_id; context->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; - // No custom handling of RMW_DEFAULT_DOMAIN_ID. Simply use a reasonable domain id. + // 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; - - const rcutils_allocator_t * allocator = &options->allocator; - - 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, 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_DEFAULT_DOMAIN_ID != options->domain_id ? options->domain_id : 0u; + + const rcutils_allocator_t *allocator = &options->allocator; + + 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, 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; if ((ret = rmw_init_options_copy(options, &context->options)) != RMW_RET_OK) { // error already set return ret; } - auto free_options = rcpputils::make_scope_exit( - [context]() { - 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"); - } - }); + auto free_options = rcpputils::make_scope_exit([context]() { + 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"); + } + }); // Set the enclave. context->impl->enclave = rcutils_strdup(options->enclave, *allocator); - RMW_CHECK_FOR_NULL_WITH_MSG( - context->impl->enclave, - "failed to allocate enclave", - return RMW_RET_BAD_ALLOC); - auto free_enclave = rcpputils::make_scope_exit( - [context, allocator]() { - allocator->deallocate(context->impl->enclave, allocator->state); - }); + RMW_CHECK_FOR_NULL_WITH_MSG(context->impl->enclave, + "failed to allocate enclave", + return RMW_RET_BAD_ALLOC); + auto free_enclave = rcpputils::make_scope_exit([context, allocator]() { + allocator->deallocate(context->impl->enclave, allocator->state); + }); // Initialize context's implementation context->impl->is_shutdown = false; @@ -178,32 +167,30 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // Initialize the zenoh configuration. z_owned_config_t config; - if ((ret = - rmw_zenoh_cpp::get_z_config( - rmw_zenoh_cpp::ConfigurableEntity::Session, - &config)) != RMW_RET_OK) - { + if ((ret = rmw_zenoh_cpp::get_z_config( + rmw_zenoh_cpp::ConfigurableEntity::Session, &config)) != + RMW_RET_OK) { RMW_SET_ERROR_MSG("Error configuring Zenoh session."); return ret; } - // 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)); - }); + // // TODO(yuyuan): SHM is disabled + // // 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)); + z_open(&context->impl->session, z_move(config)); if (!z_session_check(&context->impl->session)) { RMW_SET_ERROR_MSG("Error setting up zenoh session"); return RMW_RET_ERROR; } auto close_session = rcpputils::make_scope_exit( - [context]() { - z_close(z_move(context->impl->session)); - }); + [context]() { z_close(z_move(context->impl->session)); }); /// Initialize the graph cache. z_id_t zid = z_info_zid(z_loan(context->impl->session)); @@ -211,146 +198,156 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // Verify if the zenoh router is running if configured. const std::optional configured_connection_attempts = - rmw_zenoh_cpp::zenoh_router_check_attempts(); + rmw_zenoh_cpp::zenoh_router_check_attempts(); if (configured_connection_attempts.has_value()) { ret = RMW_RET_ERROR; uint64_t connection_attempts = 0; // Retry until the connection is successful. - while (ret != RMW_RET_OK && connection_attempts < configured_connection_attempts.value()) { - if ((ret = rmw_zenoh_cpp::zenoh_router_check(z_loan(context->impl->session))) != RMW_RET_OK) { + while (ret != RMW_RET_OK && + connection_attempts < configured_connection_attempts.value()) { + if ((ret = rmw_zenoh_cpp::zenoh_router_check( + z_loan(context->impl->session))) != RMW_RET_OK) { ++connection_attempts; } std::this_thread::sleep_for(std::chrono::seconds(1)); } if (ret != RMW_RET_OK) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Unable to connect to a Zenoh router after %zu retries.", - configured_connection_attempts.value()); + "Unable to connect to a Zenoh router after %zu retries.", + configured_connection_attempts.value()); return ret; } } - // Initialize the shm manager if shared_memory is enabled in the config. - if (shm_enabled._cstr != nullptr && - strcmp(shm_enabled._cstr, "true") == 0) - { - char idstr[sizeof(zid.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(zid.id); ++i) { - snprintf(idstr + 2 * i, max_size_of_each, "%02x", zid.id[i]); - } - idstr[sizeof(zid.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 = - zc_shm_manager_new( - z_loan(context->impl->session), - idstr, - SHM_BUFFER_SIZE_MB * 1024 * 1024); - if (!context->impl->shm_manager.has_value() || - !zc_shm_manager_check(&context->impl->shm_manager.value())) - { - RMW_SET_ERROR_MSG("Unable to create shm manager."); - return RMW_RET_ERROR; - } - } - auto free_shm_manager = rcpputils::make_scope_exit( - [context]() { - if (context->impl->shm_manager.has_value()) { - z_drop(z_move(context->impl->shm_manager.value())); - } - }); + // // TODO(yuyuan): SHM is disabled + // // Initialize the shm manager if shared_memory is enabled in the config. + // if (shm_enabled._cstr != nullptr && + // strcmp(shm_enabled._cstr, "true") == 0) + // { + // char idstr[sizeof(zid.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(zid.id); ++i) { + // snprintf(idstr + 2 * i, max_size_of_each, "%02x", zid.id[i]); + // } + // idstr[sizeof(zid.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 = + // zc_shm_manager_new( + // z_loan(context->impl->session), + // idstr, + // SHM_BUFFER_SIZE_MB * 1024 * 1024); + // if (!context->impl->shm_manager.has_value() || + // !zc_shm_manager_check(&context->impl->shm_manager.value())) + // { + // RMW_SET_ERROR_MSG("Unable to create shm manager."); + // return RMW_RET_ERROR; + // } + // } + // auto free_shm_manager = rcpputils::make_scope_exit( + // [context]() { + // if (context->impl->shm_manager.has_value()) { + // z_drop(z_move(context->impl->shm_manager.value())); + // } + // }); // Initialize the guard condition. 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 free_guard_condition = rcpputils::make_scope_exit( - [context, allocator]() { - allocator->deallocate(context->impl->graph_guard_condition, allocator->state); - }); + 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 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_cpp::rmw_zenoh_identifier; - - context->impl->graph_guard_condition->data = - allocator->zero_allocate(1, sizeof(rmw_zenoh_cpp::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); - }); - - RMW_TRY_PLACEMENT_NEW( - context->impl->graph_guard_condition->data, - context->impl->graph_guard_condition->data, - return RMW_RET_BAD_ALLOC, - rmw_zenoh_cpp::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(), - rmw_zenoh_cpp::GuardCondition); - }); + rmw_zenoh_cpp::rmw_zenoh_identifier; + + context->impl->graph_guard_condition->data = allocator->zero_allocate( + 1, sizeof(rmw_zenoh_cpp::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); + }); + + RMW_TRY_PLACEMENT_NEW(context->impl->graph_guard_condition->data, + context->impl->graph_guard_condition->data, + return RMW_RET_BAD_ALLOC, + rmw_zenoh_cpp::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(), + rmw_zenoh_cpp::GuardCondition); + }); // Setup liveliness subscriptions for discovery. - const std::string liveliness_str = rmw_zenoh_cpp::liveliness::subscription_token( - context->actual_domain_id); + const std::string liveliness_str = + rmw_zenoh_cpp::liveliness::subscription_token(context->actual_domain_id); - // Query router/liveliness participants to get graph information before this session was started. + // Query router/liveliness participants to get graph information before this + // session was started. // We create a blocking channel that is unbounded, ie. `bound` = 0, to receive - // replies for the zc_liveliness_get() call. This is necessary as if the `bound` - // is too low, the channel may starve the zenoh executor of its threads which - // would lead to deadlocks when trying to receive replies and block the - // execution here. - // The blocking channel will return when the sender end is closed which is - // the moment the query finishes. - // The non-blocking fifo exists only for the use case where we don't want to - // block the thread between responses (including the request termination response). - // In general, unless we want to cooperatively schedule other tasks on the same - // thread as reading the fifo, the blocking fifo will be more appropriate as - // the code will be simpler, and if we're just going to spin over the non-blocking - // reads until we obtain responses, we'll just be hogging CPU time by convincing - // the OS that we're doing actual work when it could instead park the thread. - z_owned_reply_channel_t channel = zc_reply_fifo_new(0); - zc_liveliness_get( - z_loan(context->impl->session), z_keyexpr(liveliness_str.c_str()), - z_move(channel.send), NULL); - z_owned_reply_t reply = z_reply_null(); - for (bool call_success = z_call(channel.recv, &reply); !call_success || z_check(reply); - call_success = z_call(channel.recv, &reply)) - { - if (!call_success) { - continue; - } - 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); + // replies for the zc_liveliness_get() call. This is necessary as if the + // `bound` is too low, the channel may starve the zenoh executor of its + // threads which would lead to deadlocks when trying to receive replies and + // block the execution here. The blocking channel will return when the sender + // end is closed which is the moment the query finishes. The non-blocking fifo + // exists only for the use case where we don't want to block the thread + // between responses (including the request termination response). In general, + // unless we want to cooperatively schedule other tasks on the same thread as + // reading the fifo, the blocking fifo will be more appropriate as the code + // will be simpler, and if we're just going to spin over the non-blocking + // reads until we obtain responses, we'll just be hogging CPU time by + // convincing the OS that we're doing actual work when it could instead park + // the thread. + z_owned_fifo_handler_reply_t handler; + z_owned_closure_reply_t closure; + // TODO(yuyuan): This one needs to be inifinite + z_fifo_channel_reply_new(&closure, &handler, 100000); + + // TODO(yuyuan): improve this + z_owned_keyexpr_t keyexpr; + z_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); + // z_view_keyexpr_t keyexpr; + // z_view_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); + zc_liveliness_get(z_loan(context->impl->session), z_loan(keyexpr), + z_move(closure), NULL); + z_owned_reply_t reply; + + + while (z_recv(z_loan(handler), &reply)) { + if (z_reply_is_ok(z_loan(reply))) { + const z_loaned_sample_t* sample = z_reply_ok(z_loan(reply)); + z_view_string_t key_str; + z_keyexpr_as_view_string(z_sample_keyexpr(sample), &key_str); + std::string str(z_string_data(z_loan(key_str)), z_string_len(z_loan(key_str))); // Ignore tokens from the same session to avoid race conditions from this // query and the liveliness subscription. - context->impl->graph_cache->parse_put(z_loan(keystr), true); - z_drop(z_move(keystr)); + context->impl->graph_cache->parse_put(str, true); } 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. + // TODO(Yadunund): Switch this to a liveliness subscriptions once the API is + // available. - // Uncomment and rely on #if #endif blocks to enable this feature when building with - // zenoh-pico since liveliness is only available in zenoh-c. + // Uncomment and rely on #if #endif blocks to enable this feature when + // building with zenoh-pico since liveliness is only available in zenoh-c. // auto sub_options = z_subscriber_options_default(); // sub_options.reliability = Z_RELIABILITY_RELIABLE; // context->impl->graph_subscriber = z_declare_subscriber( @@ -358,18 +355,28 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // z_keyexpr(liveliness_str.c_str()), // z_move(callback), // &sub_options); - auto sub_options = zc_liveliness_subscriber_options_null(); - z_owned_closure_sample_t callback = z_closure(graph_sub_data_handler, nullptr, context->impl); - context->impl->graph_subscriber = zc_liveliness_declare_subscriber( - z_loan(context->impl->session), - z_keyexpr(liveliness_str.c_str()), - z_move(callback), - &sub_options); - zc_liveliness_subscriber_options_drop(z_move(sub_options)); - auto undeclare_z_sub = rcpputils::make_scope_exit( - [context]() { - z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); - }); + zc_liveliness_subscriber_options_t sub_options; + zc_liveliness_subscriber_options_default(&sub_options); + z_owned_closure_sample_t callback; + z_closure(&callback, graph_sub_data_handler, nullptr, context->impl); + + // TODO(yuyuan): improve this + z_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); + // z_view_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); + + zc_liveliness_declare_subscriber( + &context->impl->graph_subscriber, + z_loan(context->impl->session), z_loan(keyexpr), + z_move(callback), &sub_options); + + const z_loaned_keyexpr_t *sub_ke = z_subscriber_keyexpr(z_loan(context->impl->graph_subscriber)); + z_view_string_t sub_keyexpr; + z_keyexpr_as_view_string(sub_ke, &sub_keyexpr); + + + 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; @@ -385,7 +392,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) free_options.cancel(); impl_destructor.cancel(); free_impl.cancel(); - free_shm_manager.cancel(); + // free_shm_manager.cancel(); restore_context.cancel(); return RMW_RET_OK; @@ -393,24 +400,18 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) //============================================================================== /// Shutdown the middleware for a given context. -rmw_ret_t -rmw_shutdown(rmw_context_t * 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_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + 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_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); - if (context->impl->shm_manager.has_value()) { - z_drop(z_move(context->impl->shm_manager.value())); - } + // if (context->impl->shm_manager.has_value()) { + // z_drop(z_move(context->impl->shm_manager.value())); + // } // Close the zenoh session if (z_close(z_move(context->impl->session)) < 0) { RMW_SET_ERROR_MSG("Error while closing zenoh session"); @@ -424,38 +425,34 @@ rmw_shutdown(rmw_context_t * context) //============================================================================== /// Finalize a context. -rmw_ret_t -rmw_context_fini(rmw_context_t * 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_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + 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_cpp::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; } - const rcutils_allocator_t * allocator = &context->options.allocator; + const rcutils_allocator_t *allocator = &context->options.allocator; - RMW_TRY_DESTRUCTOR( - static_cast( - context->impl->graph_guard_condition->data)->~GuardCondition(), - rmw_zenoh_cpp::GuardCondition, ); - allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); + RMW_TRY_DESTRUCTOR(static_cast( + context->impl->graph_guard_condition->data) + ->~GuardCondition(), + rmw_zenoh_cpp::GuardCondition, ); + allocator->deallocate(context->impl->graph_guard_condition->data, + allocator->state); allocator->deallocate(context->impl->graph_guard_condition, allocator->state); context->impl->graph_guard_condition = nullptr; allocator->deallocate(context->impl->enclave, allocator->state); - RMW_TRY_DESTRUCTOR(context->impl->~rmw_context_impl_t(), rmw_context_impl_t *, ); + RMW_TRY_DESTRUCTOR(context->impl->~rmw_context_impl_t(), + rmw_context_impl_t *, ); allocator->deallocate(context->impl, allocator->state); @@ -465,4 +462,4 @@ rmw_context_fini(rmw_context_t * context) return ret; } -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 3de285a5..0b69771c 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 #include #include @@ -26,11 +27,12 @@ #include #include #include +#include #include "detail/attachment_helpers.hpp" #include "detail/cdr.hpp" -#include "detail/guard_condition.hpp" #include "detail/graph_cache.hpp" +#include "detail/guard_condition.hpp" #include "detail/identifier.hpp" #include "detail/liveliness_utils.hpp" #include "detail/logging_macros.hpp" @@ -50,19 +52,18 @@ #include "rmw/dynamic_message_type_support.h" #include "rmw/error_handling.h" #include "rmw/features.h" +#include "rmw/get_topic_endpoint_info.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" -namespace -{ +namespace { //============================================================================== // Helper function to create a copy of a string after removing any // leading or trailing slashes. -std::string strip_slashes(const char * const str) -{ +std::string strip_slashes(const char *const str) { std::string ret = std::string(str); const std::size_t len = strlen(str); std::size_t start = 0; @@ -77,20 +78,17 @@ std::string strip_slashes(const char * const str) } //============================================================================== -// A function that generates a key expression for message transport of the format -// /// -// 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. -z_owned_keyexpr_t ros_topic_name_to_zenoh_key( - const std::size_t domain_id, - const char * const topic_name, - const char * const topic_type, - const char * const type_hash) -{ +// A function that generates a key expression for message transport of the +// format /// 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. +z_owned_keyexpr_t ros_topic_name_to_zenoh_key(const std::size_t domain_id, + const char *const topic_name, + const char *const topic_type, + const char *const type_hash) { std::string keyexpr_str = std::to_string(domain_id); keyexpr_str += "/"; keyexpr_str += strip_slashes(topic_name); @@ -99,28 +97,32 @@ z_owned_keyexpr_t ros_topic_name_to_zenoh_key( keyexpr_str += "/"; keyexpr_str += type_hash; - return z_keyexpr_new(keyexpr_str.c_str()); + // TODO(yuyuan): use z_view_keyexpr_t? + z_owned_keyexpr_t keyexpr; + z_keyexpr_from_str(&keyexpr, keyexpr_str.c_str()); + return keyexpr; } //============================================================================== -const rosidl_message_type_support_t * find_message_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_ZENOH_CPP_TYPESUPPORT_C); +const rosidl_message_type_support_t * +find_message_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_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); + 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); + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); return nullptr; } } @@ -129,25 +131,25 @@ const rosidl_message_type_support_t * find_message_type_support( } //============================================================================== -const rosidl_service_type_support_t * find_service_type_support( - const rosidl_service_type_support_t * type_supports) -{ - const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( - type_supports, RMW_ZENOH_CPP_TYPESUPPORT_C); +const rosidl_service_type_support_t * +find_service_type_support(const rosidl_service_type_support_t *type_supports) { + const rosidl_service_type_support_t *type_support = + get_service_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_service_typesupport_handle( - type_supports, RMW_ZENOH_CPP_TYPESUPPORT_CPP); + 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); + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); return nullptr; } } @@ -155,65 +157,49 @@ const rosidl_service_type_support_t * find_service_type_support( return type_support; } -} // namespace +} // namespace -extern "C" -{ +extern "C" { //============================================================================== /// Get the name of the rmw implementation being used -const char * -rmw_get_implementation_identifier(void) -{ +const char *rmw_get_implementation_identifier(void) { return rmw_zenoh_cpp::rmw_zenoh_identifier; } //============================================================================== /// Get the unique serialization format for this middleware. -const char * -rmw_get_serialization_format(void) -{ +const char *rmw_get_serialization_format(void) { return rmw_zenoh_cpp::rmw_zenoh_serialization_format; } //============================================================================== -bool rmw_feature_supported(rmw_feature_t feature) -{ +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; + 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 * -rmw_create_node( - rmw_context_t * context, - const char * name, - const char * namespace_) -{ +rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, + const char *namespace_) { RMW_CHECK_ARGUMENT_FOR_NULL(context, nullptr); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - context, - context->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return nullptr); - RMW_CHECK_FOR_NULL_WITH_MSG( - context->impl, - "expected initialized context", - return nullptr); - RMW_CHECK_FOR_NULL_WITH_MSG( - context->impl->enclave, - "expected initialized enclave", - return nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(context, context->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(context->impl, "expected initialized context", + return nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(context->impl->enclave, + "expected initialized enclave", return nullptr); if (context->impl->is_shutdown) { RCUTILS_SET_ERROR_MSG("context has been shutdown"); return nullptr; @@ -225,7 +211,8 @@ rmw_create_node( return nullptr; } if (RMW_NODE_NAME_VALID != validation_result) { - const char * reason = rmw_node_name_validation_result_string(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; } @@ -236,43 +223,36 @@ rmw_create_node( return nullptr; } if (RMW_NAMESPACE_VALID != validation_result) { - const char * reason = rmw_namespace_validation_result_string(validation_result); + const char *reason = + rmw_namespace_validation_result_string(validation_result); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid node namespace: %s", reason); return nullptr; } - rcutils_allocator_t * allocator = &context->options.allocator; + 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); + 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 free_node = rcpputils::make_scope_exit( - [node, allocator]() { - allocator->deallocate(node, allocator->state); - }); + [node, allocator]() { allocator->deallocate(node, allocator->state); }); 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->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_ = 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); - }); + 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); + }); // Put metadata into node->data. auto node_data = static_cast( @@ -298,16 +278,38 @@ rmw_create_node( // Initialize liveliness token for the node to advertise that a new node is in town. node_data->id = context->impl->get_next_entity_id(); node_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(z_loan(context->impl->session)), - std::to_string(node_data->id), - std::to_string(node_data->id), - rmw_zenoh_cpp::liveliness::EntityType::Node, - rmw_zenoh_cpp::liveliness::NodeInfo{context->actual_domain_id, namespace_, name, - context->impl->enclave}); + z_info_zid(z_loan(context->impl->session)), std::to_string(node_data->id), + std::to_string(node_data->id), + rmw_zenoh_cpp::liveliness::EntityType::Node, + rmw_zenoh_cpp::liveliness::NodeInfo{context->actual_domain_id, namespace_, + name, context->impl->enclave}); if (node_data->entity == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to generate keyexpr for liveliness token for the node."); + "rmw_zenoh_cpp", + "Unable to generate keyexpr for liveliness token for the node."); + return nullptr; + } + + z_owned_keyexpr_t keyexpr; + z_error_t z_ret = + z_keyexpr_from_str(&keyexpr, node_data->entity->keyexpr().c_str()); + // WARN(yuyuan): z_view_keyexpr_t would fail + // z_view_keyexpr_t keyexpr; + // z_error_t z_ret = + // z_view_keyexpr_from_str(&keyexpr, + // node_data->entity->keyexpr().c_str()); + if (z_ret) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Unable to generate keyexpr from the entity string."); + return nullptr; + } + + z_ret = zc_liveliness_declare_token(&node_data->token, + z_loan(node->context->impl->session), + z_loan(keyexpr), NULL); + if (z_ret) { + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", + "Failed to declare liveness token."); return nullptr; } node_data->token = zc_liveliness_declare_token( @@ -316,13 +318,10 @@ rmw_create_node( NULL ); auto free_token = rcpputils::make_scope_exit( - [node_data]() { - z_drop(z_move(node_data->token)); - }); + [node_data]() { z_drop(z_move(node_data->token)); }); if (!z_check(node_data->token)) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to create liveliness token for the node."); + "rmw_zenoh_cpp", "Unable to create liveliness token for the node."); return nullptr; } @@ -340,19 +339,16 @@ rmw_create_node( } //============================================================================== -/// Finalize a given node handle, reclaim the resources, and deallocate the node handle. -rmw_ret_t -rmw_destroy_node(rmw_node_t * node) -{ +/// Finalize a given node handle, reclaim the resources, and deallocate the node +/// handle. +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_ARGUMENT_FOR_NULL(node->data, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -376,14 +372,11 @@ rmw_destroy_node(rmw_node_t * node) //============================================================================== /// 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) -{ +rmw_node_get_graph_guard_condition(const rmw_node_t *node) { RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(node->context, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, nullptr); @@ -393,12 +386,10 @@ rmw_node_get_graph_guard_condition(const rmw_node_t * node) //============================================================================== /// 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) -{ +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) { static_cast(type_support); static_cast(message_bounds); static_cast(allocation); @@ -409,46 +400,37 @@ rmw_init_publisher_allocation( //============================================================================== /// Destroy a publisher allocation object. rmw_ret_t -rmw_fini_publisher_allocation( - rmw_publisher_allocation_t * allocation) -{ +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; } -namespace -{ -void -generate_random_gid(uint8_t gid[RMW_GID_STORAGE_SIZE]) -{ +namespace { +void generate_random_gid(uint8_t gid[RMW_GID_STORAGE_SIZE]) { std::random_device dev; std::mt19937 rng(dev()); std::uniform_int_distribution dist( - std::numeric_limits::min(), std::numeric_limits::max()); + std::numeric_limits::min(), + std::numeric_limits::max()); for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; ++i) { gid[i] = dist(rng); } } -} // namespace +} // namespace //============================================================================== /// 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_supports, - const char * topic_name, - const rmw_qos_profile_t * qos_profile, - const rmw_publisher_options_t * publisher_options) -{ +rmw_publisher_t *rmw_create_publisher( + const rmw_node_t *node, 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) { + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); if (topic_name[0] == '\0') { @@ -458,99 +440,93 @@ rmw_create_publisher( 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); + 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); + 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; } } 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"); + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED) { + RMW_SET_ERROR_MSG("Strict requirement on unique network flow endpoints for " + "publishers not supported"); return nullptr; } - const rmw_zenoh_cpp::rmw_node_data_t * node_data = - static_cast(node->data); + const rmw_zenoh_cpp::rmw_node_data_t *node_data = + static_cast(node->data); RMW_CHECK_ARGUMENT_FOR_NULL(node_data, nullptr); // Get the RMW type support. - const rosidl_message_type_support_t * type_support = find_message_type_support(type_supports); + const rosidl_message_type_support_t *type_support = + find_message_type_support(type_supports); if (type_support == nullptr) { // error was already set by find_message_type_support return nullptr; } + RMW_CHECK_FOR_NULL_WITH_MSG(node->context, "expected initialized context", + return nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(node->context, "expected initialized context", + 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); - RMW_CHECK_FOR_NULL_WITH_MSG( - context_impl->enclave, - "expected initialized enclave", - return nullptr); + 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); + RMW_CHECK_FOR_NULL_WITH_MSG(context_impl->enclave, + "expected initialized enclave", return nullptr); if (!z_check(context_impl->session)) { RMW_SET_ERROR_MSG("zenoh session is invalid"); return nullptr; } - rcutils_allocator_t * allocator = &node->context->options.allocator; + rcutils_allocator_t *allocator = &node->context->options.allocator; // Create the publisher. - 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); - auto free_rmw_publisher = rcpputils::make_scope_exit( - [rmw_publisher, allocator]() { - allocator->deallocate(rmw_publisher, allocator->state); - }); - - auto publisher_data = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::rmw_publisher_data_t), allocator->state)); - 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); - }); - - RMW_TRY_PLACEMENT_NEW( - publisher_data, publisher_data, return nullptr, - rmw_zenoh_cpp::rmw_publisher_data_t); - auto destruct_publisher_data = rcpputils::make_scope_exit( - [publisher_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - publisher_data->~rmw_publisher_data_t(), rmw_zenoh_cpp::rmw_publisher_data_t); - }); + 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); + auto free_rmw_publisher = + rcpputils::make_scope_exit([rmw_publisher, allocator]() { + allocator->deallocate(rmw_publisher, allocator->state); + }); + + auto publisher_data = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::rmw_publisher_data_t), allocator->state)); + 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); + }); + + RMW_TRY_PLACEMENT_NEW(publisher_data, publisher_data, return nullptr, + rmw_zenoh_cpp::rmw_publisher_data_t); + auto destruct_publisher_data = rcpputils::make_scope_exit([publisher_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + publisher_data->~rmw_publisher_data_t(), + rmw_zenoh_cpp::rmw_publisher_data_t); + }); generate_random_gid(publisher_data->pub_gid); // Adapt any 'best available' QoS options publisher_data->adapted_qos_profile = *qos_profile; rmw_ret_t ret = rmw_zenoh_cpp::QoS::get().best_available_qos( - node, topic_name, &publisher_data->adapted_qos_profile, rmw_get_subscriptions_info_by_topic); + node, topic_name, &publisher_data->adapted_qos_profile, + rmw_get_subscriptions_info_by_topic); if (RMW_RET_OK != ret) { return nullptr; } @@ -558,156 +534,151 @@ rmw_create_publisher( publisher_data->typesupport_identifier = type_support->typesupport_identifier; publisher_data->type_hash = type_support->get_type_hash_func(type_support); publisher_data->type_support_impl = type_support->data; - auto callbacks = static_cast(type_support->data); - publisher_data->type_support = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::MessageTypeSupport), allocator->state)); - 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); - }); - - RMW_TRY_PLACEMENT_NEW( - publisher_data->type_support, - publisher_data->type_support, - return nullptr, - rmw_zenoh_cpp::MessageTypeSupport, callbacks); - auto destruct_msg_type_support = rcpputils::make_scope_exit( - [publisher_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - publisher_data->type_support->~MessageTypeSupport(), - rmw_zenoh_cpp::MessageTypeSupport); - }); + auto callbacks = + static_cast(type_support->data); + publisher_data->type_support = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::MessageTypeSupport), allocator->state)); + 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); + }); + + RMW_TRY_PLACEMENT_NEW(publisher_data->type_support, + publisher_data->type_support, return nullptr, + rmw_zenoh_cpp::MessageTypeSupport, callbacks); + auto destruct_msg_type_support = + rcpputils::make_scope_exit([publisher_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + publisher_data->type_support->~MessageTypeSupport(), + rmw_zenoh_cpp::MessageTypeSupport); + }); publisher_data->context = node->context; rmw_publisher->data = publisher_data; - rmw_publisher->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; + rmw_publisher->implementation_identifier = + rmw_zenoh_cpp::rmw_zenoh_identifier; rmw_publisher->options = *publisher_options; rmw_publisher->can_loan_messages = false; rmw_publisher->topic_name = rcutils_strdup(topic_name, *allocator); - 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); - }); + 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); + }); // Convert the type hash to a string so that it can be included in // the keyexpr. - char * type_hash_c_str = nullptr; + char *type_hash_c_str = nullptr; rcutils_ret_t stringify_ret = rosidl_stringify_type_hash( - publisher_data->type_hash, - *allocator, - &type_hash_c_str); + publisher_data->type_hash, *allocator, &type_hash_c_str); if (RCUTILS_RET_BAD_ALLOC == stringify_ret) { RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str."); return nullptr; } - auto free_type_hash_c_str = rcpputils::make_scope_exit( - [&allocator, &type_hash_c_str]() { - allocator->deallocate(type_hash_c_str, allocator->state); - }); + auto free_type_hash_c_str = + rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { + allocator->deallocate(type_hash_c_str, allocator->state); + }); z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key( - node->context->actual_domain_id, - topic_name, - publisher_data->type_support->get_name(), - type_hash_c_str); + node->context->actual_domain_id, topic_name, + publisher_data->type_support->get_name(), type_hash_c_str); + z_view_string_t str; + z_keyexpr_as_view_string(z_loan(keyexpr), &str); auto always_free_ros_keyexpr = rcpputils::make_scope_exit( - [&keyexpr]() { - z_keyexpr_drop(z_move(keyexpr)); - }); + [&keyexpr]() { z_keyexpr_drop(z_move(keyexpr)); }); if (!z_keyexpr_check(&keyexpr)) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } // Create a Publication Cache if durability is transient_local. - if (publisher_data->adapted_qos_profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { - ze_publication_cache_options_t pub_cache_opts = ze_publication_cache_options_default(); + if (publisher_data->adapted_qos_profile.durability == + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { + ze_publication_cache_options_t pub_cache_opts; + ze_publication_cache_options_default(&pub_cache_opts); pub_cache_opts.history = publisher_data->adapted_qos_profile.depth; pub_cache_opts.queryable_complete = true; - publisher_data->pub_cache = ze_declare_publication_cache( - z_loan(context_impl->session), - z_loan(keyexpr), - &pub_cache_opts - ); - if (!publisher_data->pub_cache.has_value() || !z_check(publisher_data->pub_cache.value())) { + + ze_owned_publication_cache_t pub_cache; + if (ze_declare_publication_cache(&pub_cache, z_loan(context_impl->session), + z_loan(keyexpr), &pub_cache_opts)) { RMW_SET_ERROR_MSG("unable to create zenoh publisher cache"); return nullptr; } + publisher_data->pub_cache = pub_cache; } - auto undeclare_z_publisher_cache = rcpputils::make_scope_exit( - [publisher_data]() { - if (publisher_data && publisher_data->pub_cache.has_value()) { - z_drop(z_move(publisher_data->pub_cache.value())); - } - }); + auto undeclare_z_publisher_cache = + rcpputils::make_scope_exit([publisher_data]() { + if (publisher_data && publisher_data->pub_cache.has_value()) { + z_drop(z_move(publisher_data->pub_cache.value())); + } + }); // Set congestion_control to BLOCK if appropriate. - z_publisher_options_t opts = z_publisher_options_default(); + z_publisher_options_t opts; + z_publisher_options_default(&opts); opts.congestion_control = Z_CONGESTION_CONTROL_DROP; - if (publisher_data->adapted_qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL && - publisher_data->adapted_qos_profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) - { + if (publisher_data->adapted_qos_profile.history == + RMW_QOS_POLICY_HISTORY_KEEP_ALL && + publisher_data->adapted_qos_profile.reliability == + RMW_QOS_POLICY_RELIABILITY_RELIABLE) { opts.congestion_control = Z_CONGESTION_CONTROL_BLOCK; } - // 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_loan(keyexpr), - &opts - ); - if (!z_check(publisher_data->pub)) { + // TODO(clalancette): What happens if the key name is a valid but empty + // string? + if (z_declare_publisher(&publisher_data->pub, z_loan(context_impl->session), + z_loan(keyexpr), &opts) < 0) { 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)); - }); + auto undeclare_z_publisher = rcpputils::make_scope_exit([publisher_data]() { + z_undeclare_publisher(z_move(publisher_data->pub)); + }); publisher_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(z_loan(node->context->impl->session)), - std::to_string(node_data->id), - std::to_string( - context_impl->get_next_entity_id()), - rmw_zenoh_cpp::liveliness::EntityType::Publisher, - rmw_zenoh_cpp::liveliness::NodeInfo{ - node->context->actual_domain_id, node->namespace_, node->name, context_impl->enclave}, - rmw_zenoh_cpp::liveliness::TopicInfo{ - rmw_publisher->topic_name, - publisher_data->type_support->get_name(), - type_hash_c_str, - publisher_data->adapted_qos_profile} - ); + z_info_zid(z_loan(node->context->impl->session)), + std::to_string(node_data->id), + std::to_string(context_impl->get_next_entity_id()), + rmw_zenoh_cpp::liveliness::EntityType::Publisher, + rmw_zenoh_cpp::liveliness::NodeInfo{node->context->actual_domain_id, + node->namespace_, node->name, + context_impl->enclave}, + rmw_zenoh_cpp::liveliness::TopicInfo{ + rmw_publisher->topic_name, publisher_data->type_support->get_name(), + type_hash_c_str, publisher_data->adapted_qos_profile}); if (publisher_data->entity == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to generate keyexpr for liveliness token for the publisher."); + "rmw_zenoh_cpp", + "Unable to generate keyexpr for liveliness token for the publisher."); return nullptr; } - publisher_data->token = zc_liveliness_declare_token( - z_loan(node->context->impl->session), - z_keyexpr(publisher_data->entity->keyexpr().c_str()), - NULL - ); - auto free_token = rcpputils::make_scope_exit( - [publisher_data]() { - if (publisher_data != nullptr) { - z_drop(z_move(publisher_data->token)); - } - }); - if (!z_check(publisher_data->token)) { + z_owned_keyexpr_t liveliness_keyexpr; + z_keyexpr_from_str(&liveliness_keyexpr, + publisher_data->entity->keyexpr().c_str()); + // WARN(yuyuan): z_view_keyexpr_t would fail + // z_view_keyexpr_t liveliness_keyexpr; + // z_view_keyexpr_from_str(&liveliness_keyexpr, + // publisher_data->entity->keyexpr().c_str()); + z_error_t z_ret = zc_liveliness_declare_token( + &publisher_data->token, z_loan(node->context->impl->session), + z_loan(liveliness_keyexpr), NULL); + auto free_token = rcpputils::make_scope_exit([publisher_data]() { + if (publisher_data != nullptr) { + z_drop(z_move(publisher_data->token)); + } + }); + if (z_ret) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to create liveliness token for the publisher."); + "rmw_zenoh_cpp", + "Unable to create liveliness token for the publisher."); return nullptr; } @@ -725,57 +696,54 @@ rmw_create_publisher( } //============================================================================== -/// 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) -{ +/// 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) { RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(publisher->data, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - publisher, - publisher->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(publisher, + publisher->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); rmw_ret_t ret = RMW_RET_OK; - rcutils_allocator_t * allocator = &node->context->options.allocator; + rcutils_allocator_t *allocator = &node->context->options.allocator; - auto publisher_data = static_cast(publisher->data); + auto publisher_data = + static_cast(publisher->data); if (publisher_data != nullptr) { zc_liveliness_undeclare_token(z_move(publisher_data->token)); if (publisher_data->pub_cache.has_value()) { z_drop(z_move(publisher_data->pub_cache.value())); } - RMW_TRY_DESTRUCTOR(publisher_data->type_support->~MessageTypeSupport(), MessageTypeSupport, ); + 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"); ret = RMW_RET_ERROR; } - RMW_TRY_DESTRUCTOR(publisher_data->~rmw_publisher_data_t(), rmw_publisher_data_t, ); + RMW_TRY_DESTRUCTOR(publisher_data->~rmw_publisher_data_t(), + rmw_publisher_data_t, ); allocator->deallocate(publisher_data, allocator->state); } - allocator->deallocate(const_cast(publisher->topic_name), allocator->state); + allocator->deallocate(const_cast(publisher->topic_name), + allocator->state); allocator->deallocate(publisher, allocator->state); 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) -{ +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) { static_cast(subscription); static_cast(dynamic_message); static_cast(taken); @@ -784,14 +752,11 @@ rmw_take_dynamic_message( } //============================================================================== -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) -{ +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) { static_cast(subscription); static_cast(dynamic_message); static_cast(taken); @@ -801,12 +766,9 @@ rmw_take_dynamic_message_with_info( } //============================================================================== -rmw_ret_t -rmw_serialization_support_init( - const char * serialization_lib_name, - rcutils_allocator_t * allocator, - rosidl_dynamic_typesupport_serialization_support_t * serialization_support) -{ +rmw_ret_t rmw_serialization_support_init( + const char *serialization_lib_name, rcutils_allocator_t *allocator, + rosidl_dynamic_typesupport_serialization_support_t *serialization_support) { static_cast(serialization_lib_name); static_cast(allocator); static_cast(serialization_support); @@ -816,11 +778,9 @@ rmw_serialization_support_init( //============================================================================== /// 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) -{ +rmw_borrow_loaned_message(const rmw_publisher_t *publisher, + const rosidl_message_type_support_t *type_support, + void **ros_message) { static_cast(publisher); static_cast(type_support); static_cast(ros_message); @@ -830,130 +790,172 @@ rmw_borrow_loaned_message( //============================================================================== /// 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) -{ +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; } -namespace -{ -z_owned_bytes_map_t -create_map_and_set_sequence_num(int64_t sequence_number, uint8_t gid[RMW_GID_STORAGE_SIZE]) -{ - z_owned_bytes_map_t map = z_bytes_map_new(); - if (!z_check(map)) { - RMW_SET_ERROR_MSG("failed to allocate map for sequence number"); - return z_bytes_map_null(); - } - auto free_attachment_map = rcpputils::make_scope_exit( - [&map]() { - z_bytes_map_drop(z_move(map)); - }); +namespace { +z_owned_bytes_t +create_map_and_set_sequence_num(int64_t sequence_number, + uint8_t gid[RMW_GID_STORAGE_SIZE]) { - // The largest possible int64_t number is INT64_MAX, i.e. 9223372036854775807. - // That is 19 characters long, plus one for the trailing \0, means we need 20 bytes. - char seq_id_str[20]; - if (rcutils_snprintf(seq_id_str, sizeof(seq_id_str), "%" PRId64, sequence_number) < 0) { - RMW_SET_ERROR_MSG("failed to print sequence_number into buffer"); - return z_bytes_map_null(); - } - z_bytes_map_insert_by_copy(&map, z_bytes_new("sequence_number"), z_bytes_new(seq_id_str)); + // z_owned_slice_map_t map; + // z_slice_map_new(&map); - auto now = std::chrono::system_clock::now().time_since_epoch(); - auto now_ns = std::chrono::duration_cast(now); - char source_ts_str[20]; - if (rcutils_snprintf(source_ts_str, sizeof(source_ts_str), "%" PRId64, now_ns.count()) < 0) { - RMW_SET_ERROR_MSG("failed to print sequence_number into buffer"); - return z_bytes_map_null(); - } - z_bytes_map_insert_by_copy(&map, z_bytes_new("source_timestamp"), z_bytes_new(source_ts_str)); + z_owned_bytes_t bytes; - z_bytes_t gid_bytes; - gid_bytes.len = RMW_GID_STORAGE_SIZE; - gid_bytes.start = gid; + // auto free_attachment_map = + // rcpputils::make_scope_exit([&map]() { z_drop(z_move(map)); }); - z_bytes_map_insert_by_copy(&map, z_bytes_new("source_gid"), gid_bytes); + // z_view_slice_t key, val; - free_attachment_map.cancel(); + // // The largest possible int64_t number is INT64_MAX, i.e. + // 9223372036854775807. + // // That is 19 characters long, plus one for the trailing \0, means we need + // 20 + // // bytes. + // char seq_id_str[20]; + // if (rcutils_snprintf(seq_id_str, sizeof(seq_id_str), "%" PRId64, + // sequence_number) < 0) { + // RMW_SET_ERROR_MSG("failed to print sequence_number into buffer"); + // z_bytes_null(&bytes); + // return bytes; + // } - return map; + // // printf("key: sequence_number, val: rseq_id_str= %s\n", seq_id_str); + // z_view_slice_from_str(&key, "sequence_number"); + // z_view_slice_wrap(&val, sequence_number_in_bytes, sizeof(int64_t)); + // z_view_slice_from_str(&val, seq_id_str); + // z_slice_map_insert_by_copy(z_loan_mut(map), z_loan(key), z_loan(val)); + + auto now = std::chrono::system_clock::now().time_since_epoch(); + auto now_ns = std::chrono::duration_cast(now); + int64_t source_timestamp = now_ns.count(); + + // char source_ts_str[20]; + // if (rcutils_snprintf(source_ts_str, sizeof(source_ts_str), "%" PRId64, + // now_ns.count()) < 0) { + // RMW_SET_ERROR_MSG("failed to print sequence_number into buffer"); + // z_bytes_null(&bytes); + // return bytes; + // } + + // z_view_slice_from_str(&key, "source_timestamp"); + // z_view_slice_from_str(&val, source_ts_str); + // z_slice_map_insert_by_copy(z_loan_mut(map), z_loan(key), z_loan(val)); + + rmw_zenoh_cpp::attachement_data_t data(sequence_number, source_timestamp, + gid); + if (data.serialize_to_zbytes(&bytes)) { + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", + "Failed to serialize the attachment"); + z_bytes_null(&bytes); + return bytes; + } + + // // DBG + // char gid_str[256]; + // gid_str[0] = '\0'; + // for (int i = 0; i < (int)RMW_GID_STORAGE_SIZE; i++) { + // char buffer[50]; // Temporary buffer to hold each number as a string + // sprintf(buffer, "%d, ", gid[i]); + // strcat(gid_str, buffer); // Concatenate buffer to result + // } + // + // z_view_slice_from_str(&key, "source_gid"); + // z_view_slice_wrap(&val, gid, RMW_GID_STORAGE_SIZE); + // z_slice_map_insert_by_copy(z_loan_mut(map), z_loan(key), z_loan(val)); + // + // free_attachment_map.cancel(); + // + // z_bytes_serialize_from_slice_map_copy(&bytes, z_loan(map)); + // z_drop(z_move(map)); + + return bytes; } -} // namespace +} // namespace //============================================================================== /// Publish a ROS message. -rmw_ret_t -rmw_publish( - const rmw_publisher_t * publisher, - 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_cpp::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); +rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, + rmw_publisher_allocation_t *allocation) { - auto publisher_data = static_cast(publisher->data); - RMW_CHECK_FOR_NULL_WITH_MSG( - publisher_data, "publisher_data is null", - return RMW_RET_INVALID_ARGUMENT); - - rcutils_allocator_t * allocator = &(publisher_data->context->options.allocator); + 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_cpp::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); + + rcutils_allocator_t *allocator = + &(publisher_data->context->options.allocator); // Serialize data. - size_t max_data_length = publisher_data->type_support->get_estimated_serialized_size( - ros_message, - publisher_data->type_support_impl); + size_t max_data_length = + publisher_data->type_support->get_estimated_serialized_size( + ros_message, publisher_data->type_support_impl); // To store serialized message byte array. - char * msg_bytes = nullptr; - std::optional shmbuf = std::nullopt; - auto always_free_shmbuf = rcpputils::make_scope_exit( - [&shmbuf]() { - if (shmbuf.has_value()) { - zc_shmbuf_drop(&shmbuf.value()); - } - }); - auto free_msg_bytes = rcpputils::make_scope_exit( - [&msg_bytes, allocator, &shmbuf]() { - if (msg_bytes && !shmbuf.has_value()) { - allocator->deallocate(msg_bytes, allocator->state); - } - }); - - // Get memory from SHM buffer if available. - if (publisher_data->context->impl->shm_manager.has_value() && - zc_shm_manager_check(&publisher_data->context->impl->shm_manager.value())) - { - shmbuf = zc_shm_alloc( - &publisher_data->context->impl->shm_manager.value(), - max_data_length); - if (!z_check(shmbuf.value())) { - zc_shm_gc(&publisher_data->context->impl->shm_manager.value()); - shmbuf = zc_shm_alloc(&publisher_data->context->impl->shm_manager.value(), max_data_length); - if (!z_check(shmbuf.value())) { - // 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 = reinterpret_cast(zc_shmbuf_ptr(&shmbuf.value())); - } 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); - } + char *msg_bytes = nullptr; + + // // TODO(yuyuan): disable SHM + // std::optional shmbuf = std::nullopt; + // auto always_free_shmbuf = rcpputils::make_scope_exit([&shmbuf]() { + // if (shmbuf.has_value()) { + // zc_shmbuf_drop(&shmbuf.value()); + // } + // }); + // auto free_msg_bytes = + // rcpputils::make_scope_exit([&msg_bytes, allocator, &shmbuf]() { + // if (msg_bytes && !shmbuf.has_value()) { + // allocator->deallocate(msg_bytes, allocator->state); + // } + // }); + // + // // Get memory from SHM buffer if available. + // if (publisher_data->context->impl->shm_manager.has_value() && + // zc_shm_manager_check( + // &publisher_data->context->impl->shm_manager.value())) { + // shmbuf = + // zc_shm_alloc(&publisher_data->context->impl->shm_manager.value(), + // max_data_length); + // if (!z_check(shmbuf.value())) { + // zc_shm_gc(&publisher_data->context->impl->shm_manager.value()); + // shmbuf = + // zc_shm_alloc(&publisher_data->context->impl->shm_manager.value(), + // max_data_length); + // if (!z_check(shmbuf.value())) { + // // 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 = reinterpret_cast(zc_shmbuf_ptr(&shmbuf.value())); + // } 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); + // } + // 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); // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(msg_bytes, max_data_length); @@ -961,10 +963,7 @@ rmw_publish( // Object that serializes the data rmw_zenoh_cpp::Cdr ser(fastbuffer); if (!publisher_data->type_support->serialize_ros_message( - ros_message, - ser.get_cdr(), - publisher_data->type_support_impl)) - { + ros_message, ser.get_cdr(), publisher_data->type_support_impl)) { RMW_SET_ERROR_MSG("could not serialize ROS message"); return RMW_RET_ERROR; } @@ -973,36 +972,41 @@ rmw_publish( int64_t sequence_number = publisher_data->get_next_sequence_number(); - z_owned_bytes_map_t map = - create_map_and_set_sequence_num(sequence_number, publisher_data->pub_gid); - if (!z_check(map)) { + z_owned_bytes_t attachment = + create_map_and_set_sequence_num(sequence_number, publisher_data->pub_gid); + if (!z_check(attachment)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } - auto free_attachment_map = rcpputils::make_scope_exit( - [&map]() { - z_bytes_map_drop(z_move(map)); - }); + auto free_attachment = rcpputils::make_scope_exit( + [&attachment]() { z_drop(z_move(attachment)); }); 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.attachment = z_bytes_map_as_attachment(&map); - - if (shmbuf.has_value()) { - zc_shmbuf_set_length(&shmbuf.value(), data_length); - zc_owned_payload_t payload = zc_shmbuf_into_payload(z_move(shmbuf.value())); - 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), - reinterpret_cast(msg_bytes), - data_length, - &options); - } + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + options.attachment = &attachment; + + // // TODO(yuyuan): disable SHM + // if (shmbuf.has_value()) { + // zc_shmbuf_set_length(&shmbuf.value(), data_length); + // zc_owned_payload_t payload = + // zc_shmbuf_into_payload(z_move(shmbuf.value())); 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), + // reinterpret_cast(msg_bytes), + // data_length, &options); + // } + + z_owned_bytes_t payload; + z_bytes_serialize_from_slice_copy( + &payload, reinterpret_cast(msg_bytes), data_length); + ret = z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options); if (ret) { RMW_SET_ERROR_MSG("unable to publish message"); return RMW_RET_ERROR; @@ -1013,12 +1017,9 @@ rmw_publish( //============================================================================== /// 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) -{ +rmw_ret_t rmw_publish_loaned_message(const rmw_publisher_t *publisher, + void *ros_message, + rmw_publisher_allocation_t *allocation) { static_cast(publisher); static_cast(ros_message); static_cast(allocation); @@ -1028,47 +1029,41 @@ rmw_publish_loaned_message( //============================================================================== /// 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) -{ +rmw_publisher_count_matched_subscriptions(const rmw_publisher_t *publisher, + size_t *subscription_count) { RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(publisher->data, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - publisher, - publisher->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(publisher, + publisher->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(subscription_count, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = - static_cast(publisher->data); + rmw_zenoh_cpp::rmw_publisher_data_t *pub_data = + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data->context, RMW_RET_INVALID_ARGUMENT); - rmw_context_impl_t * context_impl = static_cast(pub_data->context->impl); + rmw_context_impl_t *context_impl = + static_cast(pub_data->context->impl); RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); return context_impl->graph_cache->publisher_count_matched_subscriptions( - publisher, subscription_count); + publisher, subscription_count); } //============================================================================== /// 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) -{ +rmw_ret_t rmw_publisher_get_actual_qos(const rmw_publisher_t *publisher, + rmw_qos_profile_t *qos) { RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - publisher, - publisher->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(publisher, + publisher->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = - static_cast(publisher->data); + rmw_zenoh_cpp::rmw_publisher_data_t *pub_data = + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); *qos = pub_data->adapted_qos_profile; @@ -1077,30 +1072,29 @@ rmw_publisher_get_actual_qos( //============================================================================== /// 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) -{ +rmw_ret_t rmw_publish_serialized_message( + const rmw_publisher_t *publisher, + const rmw_serialized_message_t *serialized_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_cpp::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); + 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_cpp::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); + publisher_data, "publisher data pointer is null", return RMW_RET_ERROR); eprosima::fastcdr::FastBuffer buffer( - reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); + reinterpret_cast(serialized_message->buffer), + serialized_message->buffer_length); rmw_zenoh_cpp::Cdr ser(buffer); if (!ser.get_cdr().jump(serialized_message->buffer_length)) { RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); @@ -1109,33 +1103,30 @@ rmw_publish_serialized_message( uint64_t sequence_number = publisher_data->get_next_sequence_number(); - z_owned_bytes_map_t map = - create_map_and_set_sequence_num(sequence_number, publisher_data->pub_gid); + z_owned_bytes_t attachment = + create_map_and_set_sequence_num(sequence_number, publisher_data->pub_gid); - if (!z_check(map)) { + if (!z_check(attachment)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } - auto free_attachment_map = rcpputils::make_scope_exit( - [&map]() { - z_bytes_map_drop(z_move(map)); - }); + auto free_attachment = rcpputils::make_scope_exit( + [&attachment]() { z_drop(z_move(attachment)); }); const size_t data_length = ser.get_serialized_data_length(); // 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.attachment = z_bytes_map_as_attachment(&map); - // Returns 0 if success. - int8_t ret = z_publisher_put( - z_loan(publisher_data->pub), - serialized_message->buffer, - data_length, - &options); + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + options.attachment = &attachment; - if (ret) { + z_owned_bytes_t payload; + z_bytes_serialize_from_slice_copy(&payload, serialized_message->buffer, + data_length); + + if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options)) { RMW_SET_ERROR_MSG("unable to publish message"); return RMW_RET_ERROR; } @@ -1145,12 +1136,9 @@ rmw_publish_serialized_message( //============================================================================== /// 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) -{ +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) { static_cast(type_support); static_cast(message_bounds); static_cast(size); @@ -1158,18 +1146,15 @@ rmw_get_serialized_message_size( } //============================================================================== -/// 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) -{ - RMW_CHECK_FOR_NULL_WITH_MSG( - publisher, "publisher handle is null", - return RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_FOR_NULL_WITH_MSG( - publisher->data, "publisher data is null", - return RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = - static_cast(publisher->data); +/// 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) { + RMW_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher handle is null", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG(publisher->data, "publisher data is null", + return RMW_RET_INVALID_ARGUMENT); + rmw_zenoh_cpp::rmw_publisher_data_t *pub_data = + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); if (!zc_liveliness_token_check(&pub_data->token)) { @@ -1180,49 +1165,48 @@ rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) } //============================================================================== -/// 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) -{ +/// 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) { static_cast(publisher); static_cast(wait_timeout); - // We are not currently tracking all published data, so we don't know what data is in flight that - // we might have to wait for. Even if we did start tracking it, we don't have insight into the - // TCP stream that Zenoh is managing for us, so we couldn't guarantee this anyway. - // Just lie to the upper layers and say that everything is working as expected. - // We return OK rather than UNSUPPORTED so that certain upper-layer tests continue working. + // We are not currently tracking all published data, so we don't know what + // data is in flight that we might have to wait for. Even if we did start + // tracking it, we don't have insight into the TCP stream that Zenoh is + // managing for us, so we couldn't guarantee this anyway. Just lie to the + // upper layers and say that everything is working as expected. We return OK + // rather than UNSUPPORTED so that certain upper-layer tests continue working. return RMW_RET_OK; } //============================================================================== /// 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) -{ - const rosidl_message_type_support_t * ts = find_message_type_support(type_support); +rmw_ret_t rmw_serialize(const void *ros_message, + const rosidl_message_type_support_t *type_support, + rmw_serialized_message_t *serialized_message) { + const rosidl_message_type_support_t *ts = + find_message_type_support(type_support); if (ts == nullptr) { // error was already set by find_message_type_support return RMW_RET_ERROR; } - auto callbacks = static_cast(ts->data); + auto callbacks = + static_cast(ts->data); auto tss = rmw_zenoh_cpp::MessageTypeSupport(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) { + 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); + reinterpret_cast(serialized_message->buffer), data_length); rmw_zenoh_cpp::Cdr ser(buffer); auto ret = tss.serialize_ros_message(ros_message, ser.get_cdr(), callbacks); @@ -1233,36 +1217,35 @@ rmw_serialize( //============================================================================== /// 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) -{ - const rosidl_message_type_support_t * ts = find_message_type_support(type_support); +rmw_ret_t rmw_deserialize(const rmw_serialized_message_t *serialized_message, + const rosidl_message_type_support_t *type_support, + void *ros_message) { + const rosidl_message_type_support_t *ts = + find_message_type_support(type_support); if (ts == nullptr) { // error was already set by find_message_type_support return RMW_RET_ERROR; } - auto callbacks = static_cast(ts->data); + auto callbacks = + static_cast(ts->data); auto tss = rmw_zenoh_cpp::MessageTypeSupport(callbacks); eprosima::fastcdr::FastBuffer buffer( - reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); + reinterpret_cast(serialized_message->buffer), + serialized_message->buffer_length); rmw_zenoh_cpp::Cdr deser(buffer); - auto ret = tss.deserialize_ros_message(deser.get_cdr(), ros_message, callbacks); + auto ret = + tss.deserialize_ros_message(deser.get_cdr(), ros_message, callbacks); return ret == true ? RMW_RET_OK : RMW_RET_ERROR; } //============================================================================== /// 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) -{ +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) { // Unused in current implementation. static_cast(type_support); static_cast(message_bounds); @@ -1273,29 +1256,21 @@ rmw_init_subscription_allocation( //============================================================================== /// Destroy a publisher allocation object. rmw_ret_t -rmw_fini_subscription_allocation( - rmw_subscription_allocation_t * allocation) -{ +rmw_fini_subscription_allocation(rmw_subscription_allocation_t *allocation) { static_cast(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_supports, - const char * topic_name, - const rmw_qos_profile_t * qos_profile, - const rmw_subscription_options_t * subscription_options) -{ +rmw_subscription_t *rmw_create_subscription( + const rmw_node_t *node, 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) { RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); if (topic_name[0] == '\0') { @@ -1305,7 +1280,8 @@ rmw_create_subscription( RMW_CHECK_ARGUMENT_FOR_NULL(qos_profile, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - const rosidl_message_type_support_t * type_support = find_message_type_support(type_supports); + const rosidl_message_type_support_t *type_support = + find_message_type_support(type_supports); if (type_support == nullptr) { // error was already set by find_message_type_support return nullptr; @@ -1314,71 +1290,64 @@ rmw_create_subscription( RMW_CHECK_ARGUMENT_FOR_NULL(node->data, 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); + 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, "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); - RMW_CHECK_FOR_NULL_WITH_MSG( - context_impl->enclave, - "expected initialized enclave", - return nullptr); + 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); + RMW_CHECK_FOR_NULL_WITH_MSG(context_impl->enclave, + "expected initialized enclave", return nullptr); if (!z_check(context_impl->session)) { RMW_SET_ERROR_MSG("zenoh session is invalid"); return nullptr; } - rcutils_allocator_t * allocator = &node->context->options.allocator; + rcutils_allocator_t *allocator = &node->context->options.allocator; // Create the rmw_subscription. - 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, allocator]() { - allocator->deallocate(rmw_subscription, allocator->state); - }); - - auto sub_data = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::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_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, allocator]() { + allocator->deallocate(rmw_subscription, allocator->state); + }); + + auto sub_data = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::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_zenoh_cpp::rmw_subscription_data_t); - auto destruct_sub_data = rcpputils::make_scope_exit( - [sub_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + RMW_TRY_PLACEMENT_NEW(sub_data, sub_data, return nullptr, + rmw_zenoh_cpp::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_zenoh_cpp::rmw_subscription_data_t); - }); + }); // Adapt any 'best available' QoS options sub_data->adapted_qos_profile = *qos_profile; rmw_ret_t ret = rmw_zenoh_cpp::QoS::get().best_available_qos( - node, topic_name, &sub_data->adapted_qos_profile, rmw_get_publishers_info_by_topic); + node, topic_name, &sub_data->adapted_qos_profile, + rmw_get_publishers_info_by_topic); if (RMW_RET_OK != ret) { RMW_SET_ERROR_MSG("Failed to obtain adapted_qos_profile."); return nullptr; @@ -1387,43 +1356,40 @@ rmw_create_subscription( sub_data->typesupport_identifier = type_support->typesupport_identifier; sub_data->type_hash = type_support->get_type_hash_func(type_support); sub_data->type_support_impl = type_support->data; - auto callbacks = static_cast(type_support->data); - sub_data->type_support = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::MessageTypeSupport), allocator->state)); - 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); - }); + auto callbacks = + static_cast(type_support->data); + sub_data->type_support = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::MessageTypeSupport), allocator->state)); + 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); + }); - RMW_TRY_PLACEMENT_NEW( - sub_data->type_support, - sub_data->type_support, - return nullptr, - rmw_zenoh_cpp::MessageTypeSupport, callbacks); - auto destruct_msg_type_support = rcpputils::make_scope_exit( - [sub_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + RMW_TRY_PLACEMENT_NEW(sub_data->type_support, sub_data->type_support, + return nullptr, rmw_zenoh_cpp::MessageTypeSupport, + callbacks); + auto destruct_msg_type_support = rcpputils::make_scope_exit([sub_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( sub_data->type_support->~MessageTypeSupport(), rmw_zenoh_cpp::MessageTypeSupport); - }); + }); sub_data->context = node->context; - rmw_subscription->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; + rmw_subscription->implementation_identifier = + rmw_zenoh_cpp::rmw_zenoh_identifier; rmw_subscription->topic_name = rcutils_strdup(topic_name, *allocator); - 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_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; @@ -1431,135 +1397,139 @@ rmw_create_subscription( // Convert the type hash to a string so that it can be included in // the keyexpr. - char * type_hash_c_str = nullptr; + char *type_hash_c_str = nullptr; rcutils_ret_t stringify_ret = rosidl_stringify_type_hash( - sub_data->type_hash, - *allocator, - &type_hash_c_str); + sub_data->type_hash, *allocator, &type_hash_c_str); if (RCUTILS_RET_BAD_ALLOC == stringify_ret) { RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str."); return nullptr; } - auto free_type_hash_c_str = rcpputils::make_scope_exit( - [&allocator, &type_hash_c_str]() { - allocator->deallocate(type_hash_c_str, allocator->state); - }); + auto free_type_hash_c_str = + rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { + allocator->deallocate(type_hash_c_str, allocator->state); + }); // 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(rmw_zenoh_cpp::sub_data_handler, nullptr, sub_data); + z_owned_closure_sample_t callback; + z_closure(&callback, rmw_zenoh_cpp::sub_data_handler, nullptr, sub_data); z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key( - node->context->actual_domain_id, - topic_name, - sub_data->type_support->get_name(), - type_hash_c_str); + node->context->actual_domain_id, topic_name, + sub_data->type_support->get_name(), type_hash_c_str); auto always_free_ros_keyexpr = rcpputils::make_scope_exit( - [&keyexpr]() { - z_keyexpr_drop(z_move(keyexpr)); - }); + [&keyexpr]() { z_keyexpr_drop(z_move(keyexpr)); }); if (!z_keyexpr_check(&keyexpr)) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } - // Instantiate the subscription with suitable options depending on the - // adapted_qos_profile. - // TODO(Yadunund): Rely on a separate function to return the sub - // as we start supporting more qos settings. - z_owned_str_t owned_key_str = z_keyexpr_to_string(z_loan(keyexpr)); - auto always_drop_keystr = rcpputils::make_scope_exit( - [&owned_key_str]() { - z_drop(z_move(owned_key_str)); - }); - if (sub_data->adapted_qos_profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { - ze_querying_subscriber_options_t sub_options = ze_querying_subscriber_options_default(); + // // TODO(yuyuan): owned_key_str seems to be useless here? + // // Instantiate the subscription with suitable options depending on the + // // adapted_qos_profile. + // // TODO(Yadunund): Rely on a separate function to return the sub + // // as we start supporting more qos settings. + // z_owned_str_t owned_key_str = z_keyexpr_to_string(z_loan(keyexpr)); + // auto always_drop_keystr = rcpputils::make_scope_exit( + // [&owned_key_str]() { z_drop(z_move(owned_key_str)); }); + + if (sub_data->adapted_qos_profile.durability == + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { + ze_querying_subscriber_options_t sub_options; + ze_querying_subscriber_options_default(&sub_options); // Target all complete publication caches which are queryables. sub_options.query_target = Z_QUERY_TARGET_ALL_COMPLETE; - // We set consolidation to none as we need to receive transient local messages - // from a number of publishers. Eg: To receive TF data published over /tf_static - // by various publishers. + // We set consolidation to none as we need to receive transient local + // messages from a number of publishers. Eg: To receive TF data published + // over /tf_static by various publishers. sub_options.query_consolidation = z_query_consolidation_none(); - if (sub_data->adapted_qos_profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { + if (sub_data->adapted_qos_profile.reliability == + RMW_QOS_POLICY_RELIABILITY_RELIABLE) { sub_options.reliability = Z_RELIABILITY_RELIABLE; } - sub_data->sub = ze_declare_querying_subscriber( - z_loan(context_impl->session), - z_loan(keyexpr), - z_move(callback), - &sub_options - ); - if (!z_check(std::get(sub_data->sub))) { + + ; + + ze_owned_querying_subscriber_t *sub = + &std::get(sub_data->sub); + ze_declare_querying_subscriber(sub, z_loan(context_impl->session), + z_loan(keyexpr), z_move(callback), + &sub_options); + if (!z_check(*sub)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } } else { // Create a regular subscriber for all other durability settings. - z_subscriber_options_t sub_options = z_subscriber_options_default(); + z_subscriber_options_t sub_options; + z_subscriber_options_default(&sub_options); if (qos_profile->reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { sub_options.reliability = Z_RELIABILITY_RELIABLE; } - sub_data->sub = z_declare_subscriber( - z_loan(context_impl->session), - z_loan(keyexpr), - z_move(callback), - &sub_options - ); - if (!z_check(std::get(sub_data->sub))) { + + z_owned_subscriber_t *sub = &std::get(sub_data->sub); + z_declare_subscriber(sub, z_loan(context_impl->session), z_loan(keyexpr), + z_move(callback), &sub_options); + if (!z_check(*sub)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } } - auto undeclare_z_sub = rcpputils::make_scope_exit( - [sub_data]() { - z_owned_subscriber_t * sub = std::get_if(&sub_data->sub); - if (sub == nullptr || z_undeclare_subscriber(sub)) { + auto undeclare_z_sub = rcpputils::make_scope_exit([sub_data]() { + z_owned_subscriber_t *sub = + std::get_if(&sub_data->sub); + if (sub == nullptr || z_undeclare_subscriber(sub)) { + RMW_SET_ERROR_MSG("failed to undeclare sub"); + } else { + ze_owned_querying_subscriber_t *querying_sub = + std::get_if(&sub_data->sub); + if (querying_sub == nullptr || + ze_undeclare_querying_subscriber(querying_sub)) { RMW_SET_ERROR_MSG("failed to undeclare sub"); - } else { - ze_owned_querying_subscriber_t * querying_sub = - std::get_if(&sub_data->sub); - if (querying_sub == nullptr || ze_undeclare_querying_subscriber(querying_sub)) { - RMW_SET_ERROR_MSG("failed to undeclare sub"); - } } - }); + } + }); // Publish to the graph that a new subscription is in town sub_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(z_loan(node->context->impl->session)), - std::to_string(node_data->id), - std::to_string( - context_impl->get_next_entity_id()), - rmw_zenoh_cpp::liveliness::EntityType::Subscription, - rmw_zenoh_cpp::liveliness::NodeInfo{ - node->context->actual_domain_id, node->namespace_, node->name, context_impl->enclave}, - rmw_zenoh_cpp::liveliness::TopicInfo{ - rmw_subscription->topic_name, - sub_data->type_support->get_name(), - type_hash_c_str, - sub_data->adapted_qos_profile} - ); + z_info_zid(z_loan(node->context->impl->session)), + std::to_string(node_data->id), + std::to_string(context_impl->get_next_entity_id()), + rmw_zenoh_cpp::liveliness::EntityType::Subscription, + rmw_zenoh_cpp::liveliness::NodeInfo{node->context->actual_domain_id, + node->namespace_, node->name, + context_impl->enclave}, + rmw_zenoh_cpp::liveliness::TopicInfo{ + rmw_subscription->topic_name, sub_data->type_support->get_name(), + type_hash_c_str, sub_data->adapted_qos_profile}); if (sub_data->entity == nullptr) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to generate keyexpr for liveliness token for the subscription."); + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", + "Unable to generate keyexpr for liveliness token " + "for the subscription."); return nullptr; } - sub_data->token = zc_liveliness_declare_token( - z_loan(context_impl->session), - z_keyexpr(sub_data->entity->keyexpr().c_str()), - NULL - ); - auto free_token = rcpputils::make_scope_exit( - [sub_data]() { - if (sub_data != nullptr) { - z_drop(z_move(sub_data->token)); - } - }); + z_owned_keyexpr_t token_keyexpr; + ret = z_keyexpr_from_str(&token_keyexpr, sub_data->entity->keyexpr().c_str()); + // WARN(yuyuan): z_view_keyexpr_t would fail + // z_view_keyexpr_t token_keyexpr; + // ret = z_view_keyexpr_from_str(&token_keyexpr, + // sub_data->entity->keyexpr().c_str()); + + if (ret) { + RMW_SET_ERROR_MSG("unable to create zenoh keyexpr for liveness."); + return nullptr; + } + zc_liveliness_declare_token(&sub_data->token, z_loan(context_impl->session), + z_loan(token_keyexpr), NULL); + auto free_token = rcpputils::make_scope_exit([sub_data]() { + if (sub_data != nullptr) { + z_drop(z_move(sub_data->token)); + } + }); if (!z_check(sub_data->token)) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to create liveliness token for the subscription."); + "rmw_zenoh_cpp", + "Unable to create liveliness token for the subscription."); return nullptr; } @@ -1578,54 +1548,57 @@ rmw_create_subscription( } //============================================================================== -/// 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) -{ +/// 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) { 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_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, - subscription->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(subscription, + subscription->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); rmw_ret_t ret = RMW_RET_OK; - rcutils_allocator_t * allocator = &node->context->options.allocator; + rcutils_allocator_t *allocator = &node->context->options.allocator; - auto sub_data = static_cast(subscription->data); + auto sub_data = + static_cast(subscription->data); if (sub_data != nullptr) { // Publish to the graph that a subscription has ridden off into the sunset zc_liveliness_undeclare_token(z_move(sub_data->token)); - RMW_TRY_DESTRUCTOR(sub_data->type_support->~MessageTypeSupport(), MessageTypeSupport, ); + RMW_TRY_DESTRUCTOR(sub_data->type_support->~MessageTypeSupport(), + MessageTypeSupport, ); allocator->deallocate(sub_data->type_support, allocator->state); - z_owned_subscriber_t * sub = std::get_if(&sub_data->sub); + z_owned_subscriber_t *sub = + std::get_if(&sub_data->sub); if (sub != nullptr) { if (z_undeclare_subscriber(sub)) { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; } } else { - ze_owned_querying_subscriber_t * querying_sub = - std::get_if(&sub_data->sub); - if (querying_sub == nullptr || ze_undeclare_querying_subscriber(querying_sub)) { + ze_owned_querying_subscriber_t *querying_sub = + std::get_if(&sub_data->sub); + if (querying_sub == nullptr || + ze_undeclare_querying_subscriber(querying_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, ); + RMW_TRY_DESTRUCTOR(sub_data->~rmw_subscription_data_t(), + rmw_subscription_data_t, ); allocator->deallocate(sub_data, allocator->state); } - allocator->deallocate(const_cast(subscription->topic_name), allocator->state); + allocator->deallocate(const_cast(subscription->topic_name), + allocator->state); allocator->deallocate(subscription, allocator->state); return ret; @@ -1633,47 +1606,42 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) //============================================================================== /// 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) -{ +rmw_ret_t rmw_subscription_count_matched_publishers( + const rmw_subscription_t *subscription, size_t *publisher_count) { RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(subscription->data, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, - subscription->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(subscription, + subscription->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(publisher_count, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_subscription_data_t * sub_data = - static_cast(subscription->data); + rmw_zenoh_cpp::rmw_subscription_data_t *sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data->context, RMW_RET_INVALID_ARGUMENT); - rmw_context_impl_t * context_impl = static_cast(sub_data->context->impl); + rmw_context_impl_t *context_impl = + static_cast(sub_data->context->impl); RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); return context_impl->graph_cache->subscription_count_matched_publishers( - subscription, publisher_count); + subscription, publisher_count); } //============================================================================== /// 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) -{ +rmw_subscription_get_actual_qos(const rmw_subscription_t *subscription, + rmw_qos_profile_t *qos) { RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, - subscription->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(subscription, + subscription->implementation_identifier, + rmw_zenoh_cpp::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); + auto sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); *qos = sub_data->adapted_qos_profile; @@ -1682,11 +1650,9 @@ rmw_subscription_get_actual_qos( //============================================================================== /// 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) -{ +rmw_ret_t 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; @@ -1694,47 +1660,41 @@ rmw_subscription_set_content_filter( //============================================================================== /// 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) -{ +rmw_ret_t rmw_subscription_get_content_filter( + const rmw_subscription_t *subscription, rcutils_allocator_t *allocator, + rmw_subscription_content_filter_options_t *options) { static_cast(subscription); static_cast(allocator); static_cast(options); return RMW_RET_UNSUPPORTED; } -namespace -{ -rmw_ret_t -__rmw_take_one( - rmw_zenoh_cpp::rmw_subscription_data_t * sub_data, - void * ros_message, - rmw_message_info_t * message_info, - bool * taken) -{ +namespace { +rmw_ret_t __rmw_take_one(rmw_zenoh_cpp::rmw_subscription_data_t *sub_data, + void *ros_message, rmw_message_info_t *message_info, + bool *taken) { *taken = false; - std::unique_ptr msg_data = sub_data->pop_next_message(); + std::unique_ptr msg_data = + sub_data->pop_next_message(); if (msg_data == nullptr) { // There are no more messages to take. return RMW_RET_OK; } + z_owned_slice_t slice; + z_bytes_deserialize_into_slice(z_loan(msg_data->payload), &slice); + // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(const_cast(msg_data->payload.payload.start)), - msg_data->payload.payload.len); + reinterpret_cast( + const_cast(z_slice_data(z_loan(slice)))), + z_slice_len(z_loan(slice))); // Object that serializes the data rmw_zenoh_cpp::Cdr deser(fastbuffer); if (!sub_data->type_support->deserialize_ros_message( - deser.get_cdr(), - ros_message, - sub_data->type_support_impl)) - { + deser.get_cdr(), ros_message, sub_data->type_support_impl)) { RMW_SET_ERROR_MSG("could not deserialize ROS message"); return RMW_RET_ERROR; } @@ -1745,8 +1705,10 @@ __rmw_take_one( message_info->publication_sequence_number = msg_data->sequence_number; // TODO(clalancette): fill in reception_sequence_number message_info->reception_sequence_number = 0; - message_info->publisher_gid.implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; - memcpy(message_info->publisher_gid.data, msg_data->publisher_gid, RMW_GID_STORAGE_SIZE); + message_info->publisher_gid.implementation_identifier = + rmw_zenoh_cpp::rmw_zenoh_identifier; + memcpy(message_info->publisher_gid.data, msg_data->publisher_gid, + RMW_GID_STORAGE_SIZE); message_info->from_intra_process = false; } @@ -1754,17 +1716,12 @@ __rmw_take_one( return RMW_RET_OK; } -} // namespace +} // namespace //============================================================================== /// 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_ret_t rmw_take(const rmw_subscription_t *subscription, void *ros_message, + bool *taken, rmw_subscription_allocation_t *allocation) { static_cast(allocation); RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); @@ -1772,14 +1729,15 @@ rmw_take( 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_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(subscription handle, + subscription->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); *taken = false; - auto sub_data = static_cast(subscription->data); + auto sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); return __rmw_take_one(sub_data, ros_message, nullptr, taken); @@ -1787,14 +1745,10 @@ rmw_take( //============================================================================== /// 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) -{ +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) { static_cast(allocation); RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); @@ -1803,14 +1757,15 @@ rmw_take_with_info( RMW_CHECK_ARGUMENT_FOR_NULL(ros_message, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(subscription handle, + subscription->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); *taken = false; - auto sub_data = static_cast(subscription->data); + auto sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); return __rmw_take_one(sub_data, ros_message, message_info, taken); @@ -1818,15 +1773,12 @@ rmw_take_with_info( //============================================================================== /// 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) -{ +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) { static_cast(allocation); RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); @@ -1835,10 +1787,10 @@ rmw_take_sequence( RMW_CHECK_ARGUMENT_FOR_NULL(message_sequence, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(message_info_sequence, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(subscription handle, + subscription->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); if (0u == count) { RMW_SET_ERROR_MSG("count cannot be 0"); @@ -1857,14 +1809,15 @@ rmw_take_sequence( if (count > (std::numeric_limits::max)()) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Cannot take %zu samples at once, limit is %" PRIu32, - count, (std::numeric_limits::max)()); + "Cannot take %zu samples at once, limit is %" PRIu32, count, + (std::numeric_limits::max)()); return RMW_RET_ERROR; } *taken = 0; - auto sub_data = static_cast(subscription->data); + auto sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); if (sub_data->context->impl->is_shutdown) { @@ -1876,19 +1829,20 @@ rmw_take_sequence( while (*taken < count) { bool one_taken = false; - ret = __rmw_take_one( - sub_data, message_sequence->data[*taken], - &message_info_sequence->data[*taken], &one_taken); + ret = __rmw_take_one(sub_data, message_sequence->data[*taken], + &message_info_sequence->data[*taken], &one_taken); if (ret != RMW_RET_OK) { - // If we are taking a sequence and the 2nd take in the sequence failed, we'll report - // RMW_RET_ERROR to the caller, but we will *also* tell the caller that there are valid - // messages already taken (via the message_sequence size). It is up to the caller to deal - // with that situation appropriately. + // If we are taking a sequence and the 2nd take in the sequence failed, + // we'll report RMW_RET_ERROR to the caller, but we will *also* tell the + // caller that there are valid messages already taken (via the + // message_sequence size). It is up to the caller to deal with that + // situation appropriately. break; } if (!one_taken) { - // No error, but there was nothing left to be taken, so break out of the loop + // No error, but there was nothing left to be taken, so break out of the + // loop break; } @@ -1902,29 +1856,25 @@ rmw_take_sequence( } //============================================================================== -namespace -{ -rmw_ret_t -__rmw_take_serialized( - const rmw_subscription_t * subscription, - rmw_serialized_message_t * serialized_message, - bool * taken, - rmw_message_info_t * message_info) -{ +namespace { +rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, + rmw_serialized_message_t *serialized_message, + bool *taken, rmw_message_info_t *message_info) { 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(serialized_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_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(subscription handle, + subscription->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); *taken = false; - auto sub_data = static_cast(subscription->data); + auto sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); if (sub_data->context->impl->is_shutdown) { @@ -1933,23 +1883,27 @@ __rmw_take_serialized( // RETRIEVE SERIALIZED MESSAGE =============================================== - std::unique_ptr msg_data = sub_data->pop_next_message(); + std::unique_ptr msg_data = + sub_data->pop_next_message(); if (msg_data == nullptr) { - // This tells rcl that the check for a new message was done, but no messages have come in yet. + // This tells rcl that the check for a new message was done, but no messages + // have come in yet. return RMW_RET_OK; } - if (serialized_message->buffer_capacity < msg_data->payload.payload.len) { - rmw_ret_t ret = - rmw_serialized_message_resize(serialized_message, msg_data->payload.payload.len); + z_owned_slice_t payload; + z_bytes_deserialize_into_slice(z_loan(msg_data->payload), &payload); + + if (serialized_message->buffer_capacity < z_slice_len(z_loan(payload))) { + rmw_ret_t ret = rmw_serialized_message_resize(serialized_message, + z_slice_len(z_loan(payload))); if (ret != RMW_RET_OK) { - return ret; // Error message already set + return ret; // Error message already set } } - serialized_message->buffer_length = msg_data->payload.payload.len; - memcpy( - serialized_message->buffer, msg_data->payload.payload.start, - msg_data->payload.payload.len); + serialized_message->buffer_length = z_slice_len(z_loan(payload)); + memcpy(serialized_message->buffer, z_slice_data(z_loan(payload)), + z_slice_len(z_loan(payload))); *taken = true; @@ -1959,53 +1913,48 @@ __rmw_take_serialized( message_info->publication_sequence_number = msg_data->sequence_number; // TODO(clalancette): fill in reception_sequence_number message_info->reception_sequence_number = 0; - message_info->publisher_gid.implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; - memcpy(message_info->publisher_gid.data, msg_data->publisher_gid, RMW_GID_STORAGE_SIZE); + message_info->publisher_gid.implementation_identifier = + rmw_zenoh_cpp::rmw_zenoh_identifier; + memcpy(message_info->publisher_gid.data, msg_data->publisher_gid, + RMW_GID_STORAGE_SIZE); message_info->from_intra_process = false; } return RMW_RET_OK; } -} // namespace +} // namespace //============================================================================== /// 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) -{ +rmw_take_serialized_message(const rmw_subscription_t *subscription, + rmw_serialized_message_t *serialized_message, + bool *taken, + rmw_subscription_allocation_t *allocation) { static_cast(allocation); - return __rmw_take_serialized(subscription, serialized_message, taken, nullptr); + return __rmw_take_serialized(subscription, serialized_message, taken, + nullptr); } //============================================================================== /// 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) -{ +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) { static_cast(allocation); - return __rmw_take_serialized(subscription, serialized_message, taken, message_info); + return __rmw_take_serialized(subscription, serialized_message, taken, + message_info); } //============================================================================== /// 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) -{ +rmw_ret_t rmw_take_loaned_message(const rmw_subscription_t *subscription, + void **loaned_message, bool *taken, + rmw_subscription_allocation_t *allocation) { static_cast(subscription); static_cast(loaned_message); static_cast(taken); @@ -2016,13 +1965,10 @@ rmw_take_loaned_message( //============================================================================== /// 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) -{ +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) { static_cast(subscription); static_cast(loaned_message); static_cast(taken); @@ -2033,32 +1979,25 @@ rmw_take_loaned_message_with_info( //============================================================================== /// 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) -{ +rmw_ret_t 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; } //============================================================================== -/// 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_supports, - const char * service_name, - const rmw_qos_profile_t * qos_profile) -{ +/// 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_supports, + const char *service_name, const rmw_qos_profile_t *qos_profile) { + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr); if (strlen(service_name) == 0) { @@ -2068,109 +2007,100 @@ rmw_create_client( RMW_CHECK_ARGUMENT_FOR_NULL(qos_profile, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(node->context, "expected initialized context", + return nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(node->context, "expected initialized context", + 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); - RMW_CHECK_FOR_NULL_WITH_MSG( - context_impl->enclave, - "expected initialized enclave", - return nullptr); + 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); + RMW_CHECK_FOR_NULL_WITH_MSG(context_impl->enclave, + "expected initialized enclave", return nullptr); if (!z_check(context_impl->session)) { RMW_SET_ERROR_MSG("zenoh session is invalid"); return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - const rmw_zenoh_cpp::rmw_node_data_t * node_data = - static_cast(node->data); + const rmw_zenoh_cpp::rmw_node_data_t *node_data = + static_cast(node->data); if (node_data == nullptr) { - RMW_SET_ERROR_MSG( - "Unable to create client as node data is invalid."); + RMW_SET_ERROR_MSG("Unable to create client as node data is invalid."); return nullptr; } - rcutils_allocator_t * allocator = &node->context->options.allocator; + rcutils_allocator_t *allocator = &node->context->options.allocator; // Validate service name int validation_result; - if (rmw_validate_full_topic_name(service_name, &validation_result, nullptr) != RMW_RET_OK) { + if (rmw_validate_full_topic_name(service_name, &validation_result, nullptr) != + RMW_RET_OK) { RMW_SET_ERROR_MSG("rmw_validate_full_topic_name failed"); return nullptr; } - if (validation_result != RMW_TOPIC_VALID && !qos_profile->avoid_ros_namespace_conventions) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("service name is malformed: %s", service_name); + if (validation_result != RMW_TOPIC_VALID && + !qos_profile->avoid_ros_namespace_conventions) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("service name is malformed: %s", + service_name); return nullptr; } // client data - rmw_client_t * rmw_client = static_cast(allocator->zero_allocate( - 1, - sizeof(rmw_client_t), - allocator->state)); + rmw_client_t *rmw_client = static_cast( + allocator->zero_allocate(1, sizeof(rmw_client_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - rmw_client, - "failed to allocate memory for the client", - return nullptr); + rmw_client, "failed to allocate memory for the client", return nullptr); - auto free_rmw_client = rcpputils::make_scope_exit( - [rmw_client, allocator]() { - allocator->deallocate(rmw_client, allocator->state); - }); + auto free_rmw_client = rcpputils::make_scope_exit([rmw_client, allocator]() { + allocator->deallocate(rmw_client, allocator->state); + }); - auto client_data = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::rmw_client_data_t), allocator->state)); + auto client_data = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::rmw_client_data_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - client_data, - "failed to allocate memory for client data", - return nullptr); - auto free_client_data = rcpputils::make_scope_exit( - [client_data, allocator]() { - allocator->deallocate(client_data, allocator->state); - }); - - RMW_TRY_PLACEMENT_NEW(client_data, client_data, return nullptr, rmw_zenoh_cpp::rmw_client_data_t); - auto destruct_client_data = rcpputils::make_scope_exit( - [client_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - client_data->~rmw_client_data_t(), - rmw_zenoh_cpp::rmw_client_data_t); - }); + client_data, "failed to allocate memory for client data", return nullptr); + auto free_client_data = + rcpputils::make_scope_exit([client_data, allocator]() { + allocator->deallocate(client_data, allocator->state); + }); + + RMW_TRY_PLACEMENT_NEW(client_data, client_data, return nullptr, + rmw_zenoh_cpp::rmw_client_data_t); + auto destruct_client_data = rcpputils::make_scope_exit([client_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(client_data->~rmw_client_data_t(), + rmw_zenoh_cpp::rmw_client_data_t); + }); generate_random_gid(client_data->client_gid); // Adapt any 'best available' QoS options client_data->adapted_qos_profile = *qos_profile; rmw_ret_t ret = rmw_zenoh_cpp::QoS::get().best_available_qos( - nullptr, nullptr, &client_data->adapted_qos_profile, nullptr); + nullptr, nullptr, &client_data->adapted_qos_profile, nullptr); if (RMW_RET_OK != ret) { RMW_SET_ERROR_MSG("Failed to obtain adapted_qos_profile."); return nullptr; } // Obtain the type support - const rosidl_service_type_support_t * type_support = find_service_type_support(type_supports); + const rosidl_service_type_support_t *type_support = + find_service_type_support(type_supports); if (type_support == nullptr) { // error was already set by find_service_type_support return nullptr; } - auto service_members = static_cast(type_support->data); + auto service_members = + static_cast(type_support->data); auto request_members = static_cast( - service_members->request_members_->data); + service_members->request_members_->data); auto response_members = static_cast( - service_members->response_members_->data); + service_members->response_members_->data); client_data->context = node->context; client_data->typesupport_identifier = type_support->typesupport_identifier; @@ -2179,147 +2109,136 @@ rmw_create_client( client_data->response_type_support_impl = response_members; // Request type support - client_data->request_type_support = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::RequestTypeSupport), allocator->state)); + client_data->request_type_support = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::RequestTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - client_data->request_type_support, - "Failed to allocate rmw_zenoh_cpp::RequestTypeSupport", - return nullptr); - auto free_request_type_support = rcpputils::make_scope_exit( - [client_data, allocator]() { - allocator->deallocate(client_data->request_type_support, allocator->state); - }); - - RMW_TRY_PLACEMENT_NEW( - client_data->request_type_support, - client_data->request_type_support, - return nullptr, - rmw_zenoh_cpp::RequestTypeSupport, service_members); - auto destruct_request_type_support = rcpputils::make_scope_exit( - [client_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - client_data->request_type_support->~RequestTypeSupport(), - rmw_zenoh_cpp::RequestTypeSupport); - }); + client_data->request_type_support, + "Failed to allocate rmw_zenoh_cpp::RequestTypeSupport", return nullptr); + auto free_request_type_support = rcpputils::make_scope_exit([client_data, + allocator]() { + allocator->deallocate(client_data->request_type_support, allocator->state); + }); + + RMW_TRY_PLACEMENT_NEW(client_data->request_type_support, + client_data->request_type_support, return nullptr, + rmw_zenoh_cpp::RequestTypeSupport, service_members); + auto destruct_request_type_support = + rcpputils::make_scope_exit([client_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + client_data->request_type_support->~RequestTypeSupport(), + rmw_zenoh_cpp::RequestTypeSupport); + }); // Response type support - client_data->response_type_support = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::ResponseTypeSupport), allocator->state)); + client_data->response_type_support = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::ResponseTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - client_data->response_type_support, - "Failed to allocate rmw_zenoh_cpp::ResponseTypeSupport", - return nullptr); - auto free_response_type_support = rcpputils::make_scope_exit( - [client_data, allocator]() { - allocator->deallocate(client_data->response_type_support, allocator->state); - }); - - RMW_TRY_PLACEMENT_NEW( - client_data->response_type_support, - client_data->response_type_support, - return nullptr, - rmw_zenoh_cpp::ResponseTypeSupport, service_members); - auto destruct_response_type_support = rcpputils::make_scope_exit( - [client_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - client_data->response_type_support->~ResponseTypeSupport(), - rmw_zenoh_cpp::ResponseTypeSupport); - }); + client_data->response_type_support, + "Failed to allocate rmw_zenoh_cpp::ResponseTypeSupport", return nullptr); + auto free_response_type_support = rcpputils::make_scope_exit([client_data, + allocator]() { + allocator->deallocate(client_data->response_type_support, allocator->state); + }); + + RMW_TRY_PLACEMENT_NEW(client_data->response_type_support, + client_data->response_type_support, return nullptr, + rmw_zenoh_cpp::ResponseTypeSupport, service_members); + auto destruct_response_type_support = + rcpputils::make_scope_exit([client_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + client_data->response_type_support->~ResponseTypeSupport(), + rmw_zenoh_cpp::ResponseTypeSupport); + }); // Populate the rmw_client. rmw_client->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; rmw_client->service_name = rcutils_strdup(service_name, *allocator); - RMW_CHECK_FOR_NULL_WITH_MSG( - rmw_client->service_name, - "failed to allocate client name", - return nullptr); - auto free_service_name = rcpputils::make_scope_exit( - [rmw_client, allocator]() { - allocator->deallocate(const_cast(rmw_client->service_name), allocator->state); - }); - - // Note: Service request/response types will contain a suffix Request_ or Response_. - // We remove the suffix when appending the type to the liveliness tokens for - // better reusability within GraphCache. + RMW_CHECK_FOR_NULL_WITH_MSG(rmw_client->service_name, + "failed to allocate client name", return nullptr); + auto free_service_name = + rcpputils::make_scope_exit([rmw_client, allocator]() { + allocator->deallocate(const_cast(rmw_client->service_name), + allocator->state); + }); + + // Note: Service request/response types will contain a suffix Request_ or + // Response_. We remove the suffix when appending the type to the liveliness + // tokens for better reusability within GraphCache. std::string service_type = client_data->request_type_support->get_name(); size_t suffix_substring_position = service_type.find("Request_"); if (std::string::npos != suffix_substring_position) { service_type = service_type.substr(0, suffix_substring_position); } else { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unexpected type %s for client %s. Report this bug", - service_type.c_str(), rmw_client->service_name); + "rmw_zenoh_cpp", "Unexpected type %s for client %s. Report this bug", + service_type.c_str(), rmw_client->service_name); return nullptr; } // Convert the type hash to a string so that it can be included in // the keyexpr. - char * type_hash_c_str = nullptr; + char *type_hash_c_str = nullptr; rcutils_ret_t stringify_ret = rosidl_stringify_type_hash( - client_data->type_hash, - *allocator, - &type_hash_c_str); + client_data->type_hash, *allocator, &type_hash_c_str); if (RCUTILS_RET_BAD_ALLOC == stringify_ret) { RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str."); return nullptr; } - auto free_type_hash_c_str = rcpputils::make_scope_exit( - [&allocator, &type_hash_c_str]() { - allocator->deallocate(type_hash_c_str, allocator->state); - }); + auto free_type_hash_c_str = + rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { + allocator->deallocate(type_hash_c_str, allocator->state); + }); client_data->keyexpr = ros_topic_name_to_zenoh_key( - node->context->actual_domain_id, - rmw_client->service_name, - service_type.c_str(), - type_hash_c_str); + node->context->actual_domain_id, rmw_client->service_name, + service_type.c_str(), type_hash_c_str); auto free_ros_keyexpr = rcpputils::make_scope_exit( - [client_data]() { - z_keyexpr_drop(z_move(client_data->keyexpr)); - }); + [client_data]() { z_keyexpr_drop(z_move(client_data->keyexpr)); }); if (!z_keyexpr_check(&client_data->keyexpr)) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } client_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(z_loan(node->context->impl->session)), - std::to_string(node_data->id), - std::to_string( - context_impl->get_next_entity_id()), - rmw_zenoh_cpp::liveliness::EntityType::Client, - rmw_zenoh_cpp::liveliness::NodeInfo{ - node->context->actual_domain_id, node->namespace_, node->name, context_impl->enclave}, - rmw_zenoh_cpp::liveliness::TopicInfo{ - rmw_client->service_name, - std::move(service_type), - type_hash_c_str, - client_data->adapted_qos_profile} - ); + z_info_zid(z_loan(node->context->impl->session)), + std::to_string(node_data->id), + std::to_string(context_impl->get_next_entity_id()), + rmw_zenoh_cpp::liveliness::EntityType::Client, + rmw_zenoh_cpp::liveliness::NodeInfo{node->context->actual_domain_id, + node->namespace_, node->name, + context_impl->enclave}, + rmw_zenoh_cpp::liveliness::TopicInfo{ + rmw_client->service_name, std::move(service_type), type_hash_c_str, + client_data->adapted_qos_profile}); if (client_data->entity == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to generate keyexpr for liveliness token for the client."); + "rmw_zenoh_cpp", + "Unable to generate keyexpr for liveliness token for the client."); return nullptr; } - client_data->token = zc_liveliness_declare_token( - z_loan(node->context->impl->session), - z_keyexpr(client_data->entity->keyexpr().c_str()), - NULL - ); - auto free_token = rcpputils::make_scope_exit( - [client_data]() { - if (client_data != nullptr) { - z_drop(z_move(client_data->token)); - } - }); + z_owned_keyexpr_t keyexpr; + z_keyexpr_from_str(&keyexpr, client_data->entity->keyexpr().c_str()); + zc_liveliness_declare_token(&client_data->token, + z_loan(node->context->impl->session), + z_loan(keyexpr), NULL); + // WARN(yuyuan): z_view_keyexpr_t would fail + // z_view_keyexpr_t keyexpr; + // z_view_keyexpr_from_str(&keyexpr, client_data->entity->keyexpr().c_str()); + // zc_liveliness_declare_token(&client_data->token, + // z_loan(node->context->impl->session), + // z_loan(keyexpr), NULL); + auto free_token = rcpputils::make_scope_exit([client_data]() { + if (client_data != nullptr) { + z_drop(z_move(client_data->token)); + } + }); if (!z_check(client_data->token)) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to create liveliness token for the client."); + "rmw_zenoh_cpp", "Unable to create liveliness token for the client."); return nullptr; } @@ -2341,54 +2260,47 @@ rmw_create_client( //============================================================================== /// Destroy and unregister a service client from its node. -rmw_ret_t -rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) -{ +rmw_ret_t rmw_destroy_client(rmw_node_t *node, rmw_client_t *client) { // ASSERTIONS ================================================================ RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(client->data, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(client, client->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - rcutils_allocator_t * allocator = &node->context->options.allocator; + rcutils_allocator_t *allocator = &node->context->options.allocator; - rmw_zenoh_cpp::rmw_client_data_t * client_data = - static_cast(client->data); - RMW_CHECK_FOR_NULL_WITH_MSG( - client_data, - "client implementation pointer is null.", - return RMW_RET_INVALID_ARGUMENT); + rmw_zenoh_cpp::rmw_client_data_t *client_data = + static_cast(client->data); + RMW_CHECK_FOR_NULL_WITH_MSG(client_data, + "client implementation pointer is null.", + return RMW_RET_INVALID_ARGUMENT); // CLEANUP =================================================================== z_drop(z_move(client_data->keyexpr)); zc_liveliness_undeclare_token(z_move(client_data->token)); - RMW_TRY_DESTRUCTOR( - client_data->request_type_support->~RequestTypeSupport(), rmw_zenoh_cpp::RequestTypeSupport, ); + RMW_TRY_DESTRUCTOR(client_data->request_type_support->~RequestTypeSupport(), + rmw_zenoh_cpp::RequestTypeSupport, ); allocator->deallocate(client_data->request_type_support, allocator->state); - RMW_TRY_DESTRUCTOR( - client_data->response_type_support->~ResponseTypeSupport(), rmw_zenoh_cpp::ResponseTypeSupport, - ); + RMW_TRY_DESTRUCTOR(client_data->response_type_support->~ResponseTypeSupport(), + rmw_zenoh_cpp::ResponseTypeSupport, ); allocator->deallocate(client_data->response_type_support, allocator->state); - // See the comment about the "num_in_flight" class variable in the rmw_client_data_t class for - // why we need to do this. + // See the comment about the "num_in_flight" class variable in the + // rmw_client_data_t class for why we need to do this. if (!client_data->shutdown_and_query_in_flight()) { RMW_TRY_DESTRUCTOR(client_data->~rmw_client_data_t(), rmw_client_data_t, ); allocator->deallocate(client->data, allocator->state); } - allocator->deallocate(const_cast(client->service_name), allocator->state); + allocator->deallocate(const_cast(client->service_name), + allocator->state); allocator->deallocate(client, allocator->state); return RMW_RET_OK; @@ -2396,56 +2308,49 @@ rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) //============================================================================== /// Send a ROS service request. -rmw_ret_t -rmw_send_request( - const rmw_client_t * client, - const void * ros_request, - int64_t * sequence_id) -{ +rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, + int64_t *sequence_id) { + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(client->data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - - rmw_zenoh_cpp::rmw_client_data_t * client_data = - static_cast(client->data); - RMW_CHECK_FOR_NULL_WITH_MSG( - client_data, - "Unable to retrieve client_data from client.", - RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(client, client->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + rmw_zenoh_cpp::rmw_client_data_t *client_data = + static_cast(client->data); + RMW_CHECK_FOR_NULL_WITH_MSG(client_data, + "Unable to retrieve client_data from client.", + RMW_RET_INVALID_ARGUMENT); if (client_data->is_shutdown()) { return RMW_RET_ERROR; } - rmw_context_impl_s * context_impl = static_cast( - client_data->context->impl); + rmw_context_impl_s *context_impl = + static_cast(client_data->context->impl); // Serialize data - rcutils_allocator_t * allocator = &(client_data->context->options.allocator); + rcutils_allocator_t *allocator = &(client_data->context->options.allocator); - size_t max_data_length = ( - client_data->request_type_support->get_estimated_serialized_size( - ros_request, client_data->request_type_support_impl)); + size_t max_data_length = + (client_data->request_type_support->get_estimated_serialized_size( + ros_request, client_data->request_type_support_impl)); // Init serialized message byte array - char * request_bytes = static_cast(allocator->allocate( - max_data_length, - allocator->state)); + char *request_bytes = static_cast( + allocator->allocate(max_data_length, allocator->state)); if (!request_bytes) { RMW_SET_ERROR_MSG("failed allocate request message bytes"); return RMW_RET_ERROR; } - auto free_request_bytes = rcpputils::make_scope_exit( - [request_bytes, allocator]() { - allocator->deallocate(request_bytes, allocator->state); - }); + auto free_request_bytes = + rcpputils::make_scope_exit([request_bytes, allocator]() { + allocator->deallocate(request_bytes, allocator->state); + }); // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(request_bytes, max_data_length); @@ -2453,10 +2358,7 @@ rmw_send_request( // Object that serializes the data rmw_zenoh_cpp::Cdr ser(fastbuffer); if (!client_data->request_type_support->serialize_ros_message( - ros_request, - ser.get_cdr(), - client_data->request_type_support_impl)) - { + ros_request, ser.get_cdr(), client_data->request_type_support_impl)) { return RMW_RET_ERROR; } @@ -2465,98 +2367,103 @@ rmw_send_request( *sequence_id = client_data->get_next_sequence_number(); // Send request - z_get_options_t opts = z_get_options_default(); + z_get_options_t opts; + z_get_options_default(&opts); - z_owned_bytes_map_t map = create_map_and_set_sequence_num(*sequence_id, client_data->client_gid); - if (!z_check(map)) { + z_owned_bytes_t attachment = + create_map_and_set_sequence_num(*sequence_id, client_data->client_gid); + if (!z_check(attachment)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } - auto free_attachment_map = rcpputils::make_scope_exit( - [&map]() { - z_bytes_map_drop(z_move(map)); - }); + auto free_attachment = rcpputils::make_scope_exit( + [&attachment]() { z_drop(z_move(attachment)); }); - // See the comment about the "num_in_flight" class variable in the rmw_client_data_t class for - // why we need to do this. + // See the comment about the "num_in_flight" class variable in the + // rmw_client_data_t class for why we need to do this. client_data->increment_in_flight_callbacks(); - opts.attachment = z_bytes_map_as_attachment(&map); + opts.attachment = &attachment; opts.target = Z_QUERY_TARGET_ALL_COMPLETE; - // The default timeout for a z_get query is 10 seconds and if a response is not received within - // this window, the queryable will return an invalid reply. However, it is common for actions, - // which are implemented using services, to take an extended duration to complete. Hence, we set - // the timeout_ms to the largest supported value to account for most realistic scenarios. + // The default timeout for a z_get query is 10 seconds and if a response is + // not received within this window, the queryable will return an invalid + // reply. However, it is common for actions, which are implemented using + // services, to take an extended duration to complete. Hence, we set the + // timeout_ms to the largest supported value to account for most realistic + // scenarios. opts.timeout_ms = std::numeric_limits::max(); - // Latest consolidation guarantees unicity of replies for the same key expression, - // which optimizes bandwidth. The default is "None", which imples replies may come in any order - // and any number. + // Latest consolidation guarantees unicity of replies for the same key + // expression, which optimizes bandwidth. The default is "None", which imples + // replies may come in any order and any number. opts.consolidation = z_query_consolidation_latest(); - opts.value.payload = z_bytes_t{data_length, reinterpret_cast(request_bytes)}; - z_owned_closure_reply_t zn_closure_reply = - z_closure(rmw_zenoh_cpp::client_data_handler, rmw_zenoh_cpp::client_data_drop, client_data); - z_get( - z_loan(context_impl->session), - z_loan(client_data->keyexpr), "", - z_move(zn_closure_reply), - &opts); + z_bytes_serialize_from_slice_copy( + opts.payload, reinterpret_cast(request_bytes), + data_length); + + z_owned_closure_reply_t callback; + z_closure(&callback, rmw_zenoh_cpp::client_data_handler, NULL, client_data); + + // TODO(yuyuan): z_owned_closure_reply_t zn_closure_reply is replaced with a + // moved callback + z_get(z_loan(context_impl->session), z_loan(client_data->keyexpr), "", + z_move(callback), &opts); return RMW_RET_OK; } //============================================================================== /// 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) -{ +rmw_ret_t rmw_take_response(const rmw_client_t *client, + rmw_service_info_t *request_header, + void *ros_response, bool *taken) { *taken = false; RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(client->data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_FOR_NULL_WITH_MSG( - client->service_name, "client has no service name", RMW_RET_INVALID_ARGUMENT); - - rmw_zenoh_cpp::rmw_client_data_t * client_data = - static_cast(client->data); - RMW_CHECK_FOR_NULL_WITH_MSG( - client->data, "Unable to retrieve client_data from client.", RMW_RET_INVALID_ARGUMENT); - - std::unique_ptr latest_reply = client_data->pop_next_reply(); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(client, client->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_FOR_NULL_WITH_MSG(client->service_name, + "client has no service name", + RMW_RET_INVALID_ARGUMENT); + + rmw_zenoh_cpp::rmw_client_data_t *client_data = + static_cast(client->data); + RMW_CHECK_FOR_NULL_WITH_MSG(client->data, + "Unable to retrieve client_data from client.", + RMW_RET_INVALID_ARGUMENT); + + std::unique_ptr latest_reply = + client_data->pop_next_reply(); if (latest_reply == nullptr) { - // This tells rcl that the check for a new message was done, but no messages have come in yet. + // This tells rcl that the check for a new message was done, but no messages + // have come in yet. return RMW_RET_OK; } - std::optional sample = latest_reply->get_sample(); + const z_loaned_sample_t *sample = latest_reply->get_sample(); if (!sample) { RMW_SET_ERROR_MSG("invalid reply sample"); return RMW_RET_ERROR; } + z_owned_slice_t payload; + z_bytes_deserialize_into_slice(z_sample_payload(sample), &payload); + // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(const_cast(sample->payload.start)), - sample->payload.len); + reinterpret_cast( + const_cast(z_slice_data(z_loan(payload)))), + z_slice_len(z_loan(payload))); // Object that serializes the data rmw_zenoh_cpp::Cdr deser(fastbuffer); if (!client_data->response_type_support->deserialize_ros_message( - deser.get_cdr(), - ros_response, - client_data->response_type_support_impl)) - { + deser.get_cdr(), ros_response, + client_data->response_type_support_impl)) { RMW_SET_ERROR_MSG("could not deserialize ROS response"); return RMW_RET_ERROR; } @@ -2564,23 +2471,25 @@ rmw_take_response( // Fill in the request_header request_header->request_id.sequence_number = - rmw_zenoh_cpp::get_int64_from_attachment(&sample->attachment, "sequence_number"); + rmw_zenoh_cpp::get_int64_from_attachment(z_sample_attachment(sample), + "sequence_number"); if (request_header->request_id.sequence_number < 0) { - RMW_SET_ERROR_MSG("Failed to get sequence_number from client call attachment"); + RMW_SET_ERROR_MSG( + "Failed to get sequence_number from client call attachment"); return RMW_RET_ERROR; } - request_header->source_timestamp = - rmw_zenoh_cpp::get_int64_from_attachment(&sample->attachment, "source_timestamp"); + request_header->source_timestamp = rmw_zenoh_cpp::get_int64_from_attachment( + z_sample_attachment(sample), "source_timestamp"); if (request_header->source_timestamp < 0) { - RMW_SET_ERROR_MSG("Failed to get source_timestamp from client call attachment"); + RMW_SET_ERROR_MSG( + "Failed to get source_timestamp from client call attachment"); return RMW_RET_ERROR; } if (!rmw_zenoh_cpp::get_gid_from_attachment( - &sample->attachment, - request_header->request_id.writer_guid)) - { + z_sample_attachment(sample), + request_header->request_id.writer_guid)) { RMW_SET_ERROR_MSG("Could not get client gid from attachment"); return RMW_RET_ERROR; } @@ -2597,20 +2506,16 @@ rmw_take_response( //============================================================================== /// 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) -{ +rmw_client_request_publisher_get_actual_qos(const rmw_client_t *client, + rmw_qos_profile_t *qos) { RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(client, client->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_client_data_t * client_data = - static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t *client_data = + static_cast(client->data); RMW_CHECK_ARGUMENT_FOR_NULL(client_data, RMW_RET_INVALID_ARGUMENT); *qos = client_data->adapted_qos_profile; @@ -2620,30 +2525,23 @@ rmw_client_request_publisher_get_actual_qos( //============================================================================== /// 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) -{ +rmw_client_response_subscription_get_actual_qos(const rmw_client_t *client, + rmw_qos_profile_t *qos) { // The same QoS profile is used for sending requests and receiving responses. return rmw_client_request_publisher_get_actual_qos(client, qos); } //============================================================================== -/// 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_supports, - const char * service_name, - const rmw_qos_profile_t * qos_profile) -{ +/// 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_supports, + const char *service_name, const rmw_qos_profile_t *qos_profile) { // ASSERTIONS ================================================================ RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::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)) { @@ -2653,106 +2551,98 @@ rmw_create_service( RMW_CHECK_ARGUMENT_FOR_NULL(qos_profile, nullptr); if (!qos_profile->avoid_ros_namespace_conventions) { int validation_result = RMW_TOPIC_VALID; - // TODO(francocipollone): Verify if this is the right way to validate the service name. - rmw_ret_t ret = rmw_validate_full_topic_name(service_name, &validation_result, nullptr); + // TODO(francocipollone): Verify if this is the right way to validate the + // service name. + 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); + 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_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - const rmw_zenoh_cpp::rmw_node_data_t * node_data = - static_cast(node->data); + const rmw_zenoh_cpp::rmw_node_data_t *node_data = + static_cast(node->data); if (node_data == nullptr) { - RMW_SET_ERROR_MSG( - "Unable to create service as node data is invalid."); + RMW_SET_ERROR_MSG("Unable to create service as node data is invalid."); return nullptr; } + RMW_CHECK_FOR_NULL_WITH_MSG(node->context, "expected initialized context", + return nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG(node->context, "expected initialized context", + 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); - RMW_CHECK_FOR_NULL_WITH_MSG( - context_impl->enclave, - "expected initialized enclave", - return nullptr); + 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); + RMW_CHECK_FOR_NULL_WITH_MSG(context_impl->enclave, + "expected initialized enclave", return nullptr); if (!z_check(context_impl->session)) { RMW_SET_ERROR_MSG("zenoh session is invalid"); return nullptr; } // SERVICE DATA ============================================================== - rcutils_allocator_t * allocator = &node->context->options.allocator; + rcutils_allocator_t *allocator = &node->context->options.allocator; - rmw_service_t * rmw_service = static_cast(allocator->zero_allocate( - 1, - sizeof(rmw_service_t), - allocator->state)); + rmw_service_t *rmw_service = static_cast( + allocator->zero_allocate(1, sizeof(rmw_service_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - rmw_service, - "failed to allocate memory for the service", - return nullptr); - auto free_rmw_service = rcpputils::make_scope_exit( - [rmw_service, allocator]() { - allocator->deallocate(rmw_service, allocator->state); - }); - - auto service_data = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::rmw_service_data_t), allocator->state)); - RMW_CHECK_FOR_NULL_WITH_MSG( - service_data, - "failed to allocate memory for service data", - return nullptr); - auto free_service_data = rcpputils::make_scope_exit( - [service_data, allocator]() { - allocator->deallocate(service_data, allocator->state); - }); - - RMW_TRY_PLACEMENT_NEW( - service_data, service_data, return nullptr, - rmw_zenoh_cpp::rmw_service_data_t); - auto destruct_service_data = rcpputils::make_scope_exit( - [service_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - service_data->~rmw_service_data_t(), - rmw_zenoh_cpp::rmw_service_data_t); - }); + rmw_service, "failed to allocate memory for the service", return nullptr); + auto free_rmw_service = + rcpputils::make_scope_exit([rmw_service, allocator]() { + allocator->deallocate(rmw_service, allocator->state); + }); + + auto service_data = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::rmw_service_data_t), allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG(service_data, + "failed to allocate memory for service data", + return nullptr); + auto free_service_data = + rcpputils::make_scope_exit([service_data, allocator]() { + allocator->deallocate(service_data, allocator->state); + }); + + RMW_TRY_PLACEMENT_NEW(service_data, service_data, return nullptr, + rmw_zenoh_cpp::rmw_service_data_t); + auto destruct_service_data = rcpputils::make_scope_exit([service_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(service_data->~rmw_service_data_t(), + rmw_zenoh_cpp::rmw_service_data_t); + }); // Adapt any 'best available' QoS options service_data->adapted_qos_profile = *qos_profile; rmw_ret_t ret = rmw_zenoh_cpp::QoS::get().best_available_qos( - nullptr, nullptr, &service_data->adapted_qos_profile, nullptr); + nullptr, nullptr, &service_data->adapted_qos_profile, nullptr); if (RMW_RET_OK != ret) { RMW_SET_ERROR_MSG("Failed to obtain adapted_qos_profile."); return nullptr; } // Get the RMW type support. - const rosidl_service_type_support_t * type_support = find_service_type_support(type_supports); + const rosidl_service_type_support_t *type_support = + find_service_type_support(type_supports); if (type_support == nullptr) { // error was already set by find_service_type_support return nullptr; } - auto service_members = static_cast(type_support->data); + auto service_members = + static_cast(type_support->data); auto request_members = static_cast( - service_members->request_members_->data); + service_members->request_members_->data); auto response_members = static_cast( - service_members->response_members_->data); + service_members->response_members_->data); service_data->context = node->context; service_data->typesupport_identifier = type_support->typesupport_identifier; @@ -2761,166 +2651,162 @@ rmw_create_service( service_data->response_type_support_impl = response_members; // Request type support - service_data->request_type_support = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::RequestTypeSupport), allocator->state)); + service_data->request_type_support = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::RequestTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - service_data->request_type_support, - "Failed to allocate rmw_zenoh_cpp::RequestTypeSupport", - return nullptr); + service_data->request_type_support, + "Failed to allocate rmw_zenoh_cpp::RequestTypeSupport", return nullptr); auto free_request_type_support = rcpputils::make_scope_exit( - [request_type_support = service_data->request_type_support, allocator]() { - allocator->deallocate(request_type_support, allocator->state); - }); - RMW_TRY_PLACEMENT_NEW( - service_data->request_type_support, - service_data->request_type_support, - return nullptr, - rmw_zenoh_cpp::RequestTypeSupport, service_members); - auto destruct_request_type_support = rcpputils::make_scope_exit( - [service_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - service_data->request_type_support->~RequestTypeSupport(), - rmw_zenoh_cpp::RequestTypeSupport); - }); + [request_type_support = service_data->request_type_support, allocator]() { + allocator->deallocate(request_type_support, allocator->state); + }); + RMW_TRY_PLACEMENT_NEW(service_data->request_type_support, + service_data->request_type_support, return nullptr, + rmw_zenoh_cpp::RequestTypeSupport, service_members); + auto destruct_request_type_support = + rcpputils::make_scope_exit([service_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + service_data->request_type_support->~RequestTypeSupport(), + rmw_zenoh_cpp::RequestTypeSupport); + }); // Response type support - service_data->response_type_support = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::ResponseTypeSupport), allocator->state)); + service_data->response_type_support = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::ResponseTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - service_data->response_type_support, - "Failed to allocate rmw_zenoh_cpp::ResponseTypeSupport", - return nullptr); + service_data->response_type_support, + "Failed to allocate rmw_zenoh_cpp::ResponseTypeSupport", return nullptr); auto free_response_type_support = rcpputils::make_scope_exit( - [response_type_support = service_data->response_type_support, allocator]() { - allocator->deallocate(response_type_support, allocator->state); - }); - RMW_TRY_PLACEMENT_NEW( - service_data->response_type_support, - service_data->response_type_support, - return nullptr, - rmw_zenoh_cpp::ResponseTypeSupport, service_members); - auto destruct_response_type_support = rcpputils::make_scope_exit( - [service_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - service_data->response_type_support->~ResponseTypeSupport(), - rmw_zenoh_cpp::ResponseTypeSupport); - }); + [response_type_support = service_data->response_type_support, + allocator]() { + allocator->deallocate(response_type_support, allocator->state); + }); + RMW_TRY_PLACEMENT_NEW(service_data->response_type_support, + service_data->response_type_support, return nullptr, + rmw_zenoh_cpp::ResponseTypeSupport, service_members); + auto destruct_response_type_support = + rcpputils::make_scope_exit([service_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + service_data->response_type_support->~ResponseTypeSupport(), + rmw_zenoh_cpp::ResponseTypeSupport); + }); // Populate the rmw_service. rmw_service->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; rmw_service->service_name = rcutils_strdup(service_name, *allocator); - RMW_CHECK_FOR_NULL_WITH_MSG( - rmw_service->service_name, - "failed to allocate service name", - return nullptr); - auto free_service_name = rcpputils::make_scope_exit( - [rmw_service, allocator]() { - allocator->deallocate(const_cast(rmw_service->service_name), allocator->state); - }); - - // Note: Service request/response types will contain a suffix Request_ or Response_. - // We remove the suffix when appending the type to the liveliness tokens for - // better reusability within GraphCache. + RMW_CHECK_FOR_NULL_WITH_MSG(rmw_service->service_name, + "failed to allocate service name", + return nullptr); + auto free_service_name = + rcpputils::make_scope_exit([rmw_service, allocator]() { + allocator->deallocate(const_cast(rmw_service->service_name), + allocator->state); + }); + + // Note: Service request/response types will contain a suffix Request_ or + // Response_. We remove the suffix when appending the type to the liveliness + // tokens for better reusability within GraphCache. std::string service_type = service_data->response_type_support->get_name(); size_t suffix_substring_position = service_type.find("Response_"); if (std::string::npos != suffix_substring_position) { service_type = service_type.substr(0, suffix_substring_position); } else { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unexpected type %s for service %s. Report this bug", - service_type.c_str(), rmw_service->service_name); + "rmw_zenoh_cpp", "Unexpected type %s for service %s. Report this bug", + service_type.c_str(), rmw_service->service_name); return nullptr; } // Convert the type hash to a string so that it can be included in // the keyexpr. - char * type_hash_c_str = nullptr; + char *type_hash_c_str = nullptr; rcutils_ret_t stringify_ret = rosidl_stringify_type_hash( - service_data->type_hash, - *allocator, - &type_hash_c_str); + service_data->type_hash, *allocator, &type_hash_c_str); if (RCUTILS_RET_BAD_ALLOC == stringify_ret) { RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str."); return nullptr; } - auto free_type_hash_c_str = rcpputils::make_scope_exit( - [&allocator, &type_hash_c_str]() { - allocator->deallocate(type_hash_c_str, allocator->state); - }); + auto free_type_hash_c_str = + rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { + allocator->deallocate(type_hash_c_str, allocator->state); + }); service_data->keyexpr = ros_topic_name_to_zenoh_key( - node->context->actual_domain_id, - rmw_service->service_name, - service_type.c_str(), - type_hash_c_str); - auto free_ros_keyexpr = rcpputils::make_scope_exit( - [service_data]() { - if (service_data) { - z_drop(z_move(service_data->keyexpr)); - } - }); - if (!z_check(z_loan(service_data->keyexpr))) { + node->context->actual_domain_id, rmw_service->service_name, + service_type.c_str(), type_hash_c_str); + auto free_ros_keyexpr = rcpputils::make_scope_exit([service_data]() { + if (service_data) { + z_drop(z_move(service_data->keyexpr)); + } + }); + + if (!z_keyexpr_check(&service_data->keyexpr)) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } - z_owned_closure_query_t callback = z_closure( - rmw_zenoh_cpp::service_data_handler, nullptr, - service_data); + z_owned_closure_query_t callback; + z_closure(&callback, rmw_zenoh_cpp::service_data_handler, nullptr, + service_data); // Configure the queryable to process complete queries. - z_queryable_options_t qable_options = z_queryable_options_default(); + z_queryable_options_t qable_options; + z_queryable_options_default(&qable_options); qable_options.complete = true; - service_data->qable = z_declare_queryable( - z_loan(context_impl->session), - z_loan(service_data->keyexpr), - z_move(callback), - &qable_options); + z_error_t z_ret = z_declare_queryable( + &service_data->qable, z_loan(context_impl->session), + z_loan(service_data->keyexpr), z_move(callback), &qable_options); - if (!z_check(service_data->qable)) { + if (z_ret) { RMW_SET_ERROR_MSG("unable to create zenoh queryable"); return nullptr; } auto undeclare_z_queryable = rcpputils::make_scope_exit( - [service_data]() { - z_undeclare_queryable(z_move(service_data->qable)); - }); + [service_data]() { z_undeclare_queryable(z_move(service_data->qable)); }); service_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(z_loan(node->context->impl->session)), - std::to_string(node_data->id), - std::to_string( - context_impl->get_next_entity_id()), - rmw_zenoh_cpp::liveliness::EntityType::Service, - rmw_zenoh_cpp::liveliness::NodeInfo{ - node->context->actual_domain_id, node->namespace_, node->name, context_impl->enclave}, - rmw_zenoh_cpp::liveliness::TopicInfo{ - rmw_service->service_name, - std::move(service_type), - type_hash_c_str, - service_data->adapted_qos_profile} - ); + z_info_zid(z_loan(node->context->impl->session)), + std::to_string(node_data->id), + std::to_string(context_impl->get_next_entity_id()), + rmw_zenoh_cpp::liveliness::EntityType::Service, + rmw_zenoh_cpp::liveliness::NodeInfo{node->context->actual_domain_id, + node->namespace_, node->name, + context_impl->enclave}, + rmw_zenoh_cpp::liveliness::TopicInfo{ + rmw_service->service_name, std::move(service_type), type_hash_c_str, + service_data->adapted_qos_profile}); if (service_data->entity == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to generate keyexpr for liveliness token for the service."); + "rmw_zenoh_cpp", + "Unable to generate keyexpr for liveliness token for the service."); return nullptr; } - service_data->token = zc_liveliness_declare_token( - z_loan(node->context->impl->session), - z_keyexpr(service_data->entity->keyexpr().c_str()), - NULL - ); - auto free_token = rcpputils::make_scope_exit( - [service_data]() { - if (service_data != nullptr) { - z_drop(z_move(service_data->token)); - } - }); - if (!z_check(service_data->token)) { + + z_owned_keyexpr_t keyexpr; + z_keyexpr_from_str(&keyexpr, service_data->entity->keyexpr().c_str()); + // WARN(yuyuan): z_view_keyexpr_t would fail + // z_view_keyexpr_t keyexpr; + // z_view_keyexpr_from_str(&keyexpr, service_data->entity->keyexpr().c_str()); + + z_view_string_t str; + z_keyexpr_as_view_string(z_loan(keyexpr), &str); + + z_ret = zc_liveliness_declare_token(&service_data->token, + z_loan(node->context->impl->session), + z_loan(keyexpr), NULL); + + auto free_keyexpr = + rcpputils::make_scope_exit([&keyexpr]() { z_drop(z_move(keyexpr)); }); + + auto free_token = rcpputils::make_scope_exit([service_data]() { + if (service_data != nullptr) { + z_drop(z_move(service_data->token)); + } + }); + if (z_ret) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to create liveliness token for the service."); + "rmw_zenoh_cpp", "Unable to create liveliness token for the service."); return nullptr; } @@ -2937,57 +2823,53 @@ rmw_create_service( free_ros_keyexpr.cancel(); undeclare_z_queryable.cancel(); free_token.cancel(); + free_keyexpr.cancel(); return rmw_service; } //============================================================================== /// Destroy and unregister a service server from its node. -rmw_ret_t -rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) -{ +rmw_ret_t rmw_destroy_service(rmw_node_t *node, rmw_service_t *service) { // ASSERTIONS ================================================================ RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(service->data, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(service, service->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - rcutils_allocator_t * allocator = &node->context->options.allocator; + rcutils_allocator_t *allocator = &node->context->options.allocator; - rmw_zenoh_cpp::rmw_service_data_t * service_data = - static_cast(service->data); - RMW_CHECK_FOR_NULL_WITH_MSG( - service_data, - "Unable to retrieve service_data from service", - return RMW_RET_INVALID_ARGUMENT); + rmw_zenoh_cpp::rmw_service_data_t *service_data = + static_cast(service->data); + RMW_CHECK_FOR_NULL_WITH_MSG(service_data, + "Unable to retrieve service_data from service", + return RMW_RET_INVALID_ARGUMENT); // CLEANUP ================================================================ z_drop(z_move(service_data->keyexpr)); z_undeclare_queryable(z_move(service_data->qable)); zc_liveliness_undeclare_token(z_move(service_data->token)); - RMW_TRY_DESTRUCTOR( - service_data->request_type_support->~RequestTypeSupport(), rmw_zenoh_cpp::RequestTypeSupport, ); + RMW_TRY_DESTRUCTOR(service_data->request_type_support->~RequestTypeSupport(), + rmw_zenoh_cpp::RequestTypeSupport, ); allocator->deallocate(service_data->request_type_support, allocator->state); RMW_TRY_DESTRUCTOR( - service_data->response_type_support->~ResponseTypeSupport(), rmw_zenoh_cpp::ResponseTypeSupport, - ); + service_data->response_type_support->~ResponseTypeSupport(), + rmw_zenoh_cpp::ResponseTypeSupport, ); allocator->deallocate(service_data->response_type_support, allocator->state); - RMW_TRY_DESTRUCTOR(service_data->~rmw_service_data_t(), rmw_zenoh_cpp::rmw_service_data_t, ); + RMW_TRY_DESTRUCTOR(service_data->~rmw_service_data_t(), + rmw_zenoh_cpp::rmw_service_data_t, ); allocator->deallocate(service->data, allocator->state); - allocator->deallocate(const_cast(service->service_name), allocator->state); + allocator->deallocate(const_cast(service->service_name), + allocator->state); allocator->deallocate(service, allocator->state); return RMW_RET_OK; @@ -2995,13 +2877,10 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) //============================================================================== /// 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) -{ +rmw_ret_t rmw_take_request(const rmw_service_t *service, + rmw_service_info_t *request_header, + void *ros_request, bool *taken) { + *taken = false; RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); @@ -3009,43 +2888,46 @@ rmw_take_request( RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(service, service->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_FOR_NULL_WITH_MSG( - service->service_name, "service has no service name", RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG(service->service_name, + "service has no service name", + RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_service_data_t * service_data = - static_cast(service->data); - RMW_CHECK_FOR_NULL_WITH_MSG( - service->data, "Unable to retrieve service_data from service", RMW_RET_INVALID_ARGUMENT); + rmw_zenoh_cpp::rmw_service_data_t *service_data = + static_cast(service->data); + RMW_CHECK_FOR_NULL_WITH_MSG(service->data, + "Unable to retrieve service_data from service", + RMW_RET_INVALID_ARGUMENT); - std::unique_ptr query = service_data->pop_next_query(); + std::unique_ptr query = + service_data->pop_next_query(); if (query == nullptr) { - // This tells rcl that the check for a new message was done, but no messages have come in yet. + // This tells rcl that the check for a new message was done, but no messages + // have come in yet. return RMW_RET_OK; } - const z_query_t loaned_query = query->get_query(); + const z_loaned_query_t *loaned_query = query->get_query(); - // DESERIALIZE MESSAGE ======================================================== - z_value_t payload_value = z_query_value(&loaned_query); + // DESERIALIZE MESSAGE + // ======================================================== + z_owned_slice_t payload; + z_bytes_deserialize_into_slice(z_query_payload(loaned_query), &payload); // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(const_cast(payload_value.payload.start)), - payload_value.payload.len); + reinterpret_cast( + const_cast(z_slice_data(z_loan(payload)))), + z_slice_len(z_loan(payload))); // Object that serializes the data rmw_zenoh_cpp::Cdr deser(fastbuffer); if (!service_data->request_type_support->deserialize_ros_message( - deser.get_cdr(), - ros_request, - service_data->request_type_support_impl)) - { + deser.get_cdr(), ros_request, + service_data->request_type_support_impl)) { RMW_SET_ERROR_MSG("could not deserialize ROS message"); return RMW_RET_ERROR; } @@ -3053,27 +2935,26 @@ rmw_take_request( // Fill in the request header. // Get the sequence_number out of the attachment - z_attachment_t attachment = z_query_attachment(&loaned_query); + const z_loaned_bytes_t *attachment = z_query_attachment(loaned_query); request_header->request_id.sequence_number = - rmw_zenoh_cpp::get_int64_from_attachment(&attachment, "sequence_number"); + rmw_zenoh_cpp::get_int64_from_attachment(attachment, "sequence_number"); if (request_header->request_id.sequence_number < 0) { - RMW_SET_ERROR_MSG("Failed to get sequence_number from client call attachment"); + RMW_SET_ERROR_MSG( + "Failed to get sequence_number from client call attachment"); return RMW_RET_ERROR; } - request_header->source_timestamp = rmw_zenoh_cpp::get_int64_from_attachment( - &attachment, - "source_timestamp"); + request_header->source_timestamp = + rmw_zenoh_cpp::get_int64_from_attachment(attachment, "source_timestamp"); if (request_header->source_timestamp < 0) { - RMW_SET_ERROR_MSG("Failed to get source_timestamp from client call attachment"); + RMW_SET_ERROR_MSG( + "Failed to get source_timestamp from client call attachment"); return RMW_RET_ERROR; } if (!rmw_zenoh_cpp::get_gid_from_attachment( - &attachment, - request_header->request_id.writer_guid)) - { + attachment, request_header->request_id.writer_guid)) { RMW_SET_ERROR_MSG("Could not get client GID from attachment"); return RMW_RET_ERROR; } @@ -3082,8 +2963,10 @@ rmw_take_request( auto now_ns = std::chrono::duration_cast(now); request_header->received_timestamp = now_ns.count(); - // Add this query to the map, so that rmw_send_response can quickly look it up later - if (!service_data->add_to_query_map(request_header->request_id, std::move(query))) { + // Add this query to the map, so that rmw_send_response can quickly look it up + // later + if (!service_data->add_to_query_map(request_header->request_id, + std::move(query))) { RMW_SET_ERROR_MSG("duplicate sequence number in the map"); return RMW_RET_ERROR; } @@ -3095,58 +2978,51 @@ rmw_take_request( //============================================================================== /// 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) -{ +rmw_ret_t rmw_send_response(const rmw_service_t *service, + rmw_request_id_t *request_header, + void *ros_response) { RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(service->data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(service, service->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_FOR_NULL_WITH_MSG( - service->data, - "Unable to retrieve service_data from service", - RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG(service->data, + "Unable to retrieve service_data from service", + RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_service_data_t * service_data = - static_cast(service->data); + rmw_zenoh_cpp::rmw_service_data_t *service_data = + static_cast(service->data); // Create the queryable payload std::unique_ptr query = - service_data->take_from_query_map(*request_header); + service_data->take_from_query_map(*request_header); if (query == nullptr) { // If there is no data associated with this request, the higher layers of // ROS 2 seem to expect that we just silently return with no work. return RMW_RET_OK; } - rcutils_allocator_t * allocator = &(service_data->context->options.allocator); + rcutils_allocator_t *allocator = &(service_data->context->options.allocator); - size_t max_data_length = ( - service_data->response_type_support->get_estimated_serialized_size( - ros_response, service_data->response_type_support_impl)); + size_t max_data_length = + (service_data->response_type_support->get_estimated_serialized_size( + ros_response, service_data->response_type_support_impl)); // Init serialized message byte array - char * response_bytes = static_cast(allocator->allocate( - max_data_length, - allocator->state)); + char *response_bytes = static_cast( + allocator->allocate(max_data_length, allocator->state)); if (!response_bytes) { RMW_SET_ERROR_MSG("failed to allocate response message bytes"); return RMW_RET_ERROR; } - auto free_response_bytes = rcpputils::make_scope_exit( - [response_bytes, allocator]() { - allocator->deallocate(response_bytes, allocator->state); - }); + auto free_response_bytes = + rcpputils::make_scope_exit([response_bytes, allocator]() { + allocator->deallocate(response_bytes, allocator->state); + }); // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(response_bytes, max_data_length); @@ -3154,34 +3030,28 @@ rmw_send_response( // Object that serializes the data rmw_zenoh_cpp::Cdr ser(fastbuffer); if (!service_data->response_type_support->serialize_ros_message( - ros_response, - ser.get_cdr(), - service_data->response_type_support_impl)) - { + ros_response, ser.get_cdr(), + service_data->response_type_support_impl)) { return RMW_RET_ERROR; } size_t data_length = ser.get_serialized_data_length(); - const z_query_t loaned_query = query->get_query(); - z_query_reply_options_t options = z_query_reply_options_default(); + const z_loaned_query_t *loaned_query = query->get_query(); + z_query_reply_options_t options; + z_query_reply_options_default(&options); - z_owned_bytes_map_t map = create_map_and_set_sequence_num( - request_header->sequence_number, request_header->writer_guid); - if (!z_check(map)) { + *options.attachment = create_map_and_set_sequence_num( + request_header->sequence_number, request_header->writer_guid); + if (!z_check(*options.attachment)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } - auto free_attachment_map = rcpputils::make_scope_exit( - [&map]() { - z_bytes_map_drop(z_move(map)); - }); - - options.attachment = z_bytes_map_as_attachment(&map); - - z_query_reply( - &loaned_query, z_loan(service_data->keyexpr), reinterpret_cast( - response_bytes), data_length, &options); + z_owned_bytes_t payload; + z_bytes_serialize_from_slice( + &payload, reinterpret_cast(response_bytes), data_length); + z_query_reply(loaned_query, z_loan(service_data->keyexpr), z_move(payload), + &options); return RMW_RET_OK; } @@ -3189,20 +3059,16 @@ rmw_send_response( //============================================================================== /// 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) -{ +rmw_service_request_subscription_get_actual_qos(const rmw_service_t *service, + rmw_qos_profile_t *qos) { RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(service, service->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_service_data_t * service_data = - static_cast(service->data); + rmw_zenoh_cpp::rmw_service_data_t *service_data = + static_cast(service->data); RMW_CHECK_ARGUMENT_FOR_NULL(service_data, RMW_RET_INVALID_ARGUMENT); *qos = service_data->adapted_qos_profile; @@ -3212,55 +3078,50 @@ rmw_service_request_subscription_get_actual_qos( //============================================================================== /// 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) -{ +rmw_service_response_publisher_get_actual_qos(const rmw_service_t *service, + rmw_qos_profile_t *qos) { // The same QoS profile is used for receiving requests and sending responses. return rmw_service_request_subscription_get_actual_qos(service, qos); } //============================================================================== /// Create a guard condition and return a handle to that guard condition. -rmw_guard_condition_t * -rmw_create_guard_condition(rmw_context_t * context) -{ - rcutils_allocator_t * allocator = &context->options.allocator; +rmw_guard_condition_t *rmw_create_guard_condition(rmw_context_t *context) { + 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_cpp::rmw_zenoh_identifier; + 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_cpp::rmw_zenoh_identifier; guard_condition->context = context; guard_condition->data = allocator->zero_allocate( - 1, sizeof(rmw_zenoh_cpp::GuardCondition), - allocator->state); + 1, sizeof(rmw_zenoh_cpp::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) rmw_zenoh_cpp::GuardCondition; - auto destruct_guard_condition = rcpputils::make_scope_exit( - [guard_condition]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - static_cast(guard_condition->data)->~GuardCondition(), - rmw_zenoh_cpp::GuardCondition); - }); + 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) rmw_zenoh_cpp::GuardCondition; + auto destruct_guard_condition = + rcpputils::make_scope_exit([guard_condition]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + static_cast(guard_condition->data) + ->~GuardCondition(), + rmw_zenoh_cpp::GuardCondition); + }); destruct_guard_condition.cancel(); free_guard_condition_data.cancel(); @@ -3269,16 +3130,16 @@ rmw_create_guard_condition(rmw_context_t * context) return guard_condition; } -/// 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) -{ +/// 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) { RMW_CHECK_ARGUMENT_FOR_NULL(guard_condition, RMW_RET_INVALID_ARGUMENT); - rcutils_allocator_t * allocator = &guard_condition->context->options.allocator; + rcutils_allocator_t *allocator = &guard_condition->context->options.allocator; if (guard_condition->data) { - static_cast(guard_condition->data)->~GuardCondition(); + static_cast(guard_condition->data) + ->~GuardCondition(); allocator->deallocate(guard_condition->data, allocator->state); } @@ -3289,71 +3150,62 @@ rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) //============================================================================== rmw_ret_t -rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition) -{ +rmw_trigger_guard_condition(const rmw_guard_condition_t *guard_condition) { RMW_CHECK_ARGUMENT_FOR_NULL(guard_condition, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - guard_condition, - guard_condition->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(guard_condition, + guard_condition->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - static_cast(guard_condition->data)->trigger(); + static_cast(guard_condition->data) + ->trigger(); return RMW_RET_OK; } //============================================================================== /// 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) -{ +rmw_wait_set_t *rmw_create_wait_set(rmw_context_t *context, + size_t max_conditions) { + static_cast(max_conditions); RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - context, - context->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(context, context->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return nullptr); - rcutils_allocator_t * allocator = &context->options.allocator; + 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, allocator]() { - allocator->deallocate(wait_set, allocator->state); - }); + 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, allocator]() { + allocator->deallocate(wait_set, allocator->state); + }); wait_set->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; wait_set->data = allocator->zero_allocate( - 1, sizeof(rmw_zenoh_cpp::rmw_wait_set_data_t), - allocator->state); + 1, sizeof(rmw_zenoh_cpp::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, allocator]() { - allocator->deallocate(wait_set->data, allocator->state); - }); + wait_set->data, "failed to allocate wait set data", return nullptr); + auto free_wait_set_data = rcpputils::make_scope_exit([wait_set, allocator]() { + allocator->deallocate(wait_set->data, allocator->state); + }); // Invoke placement new - new(wait_set->data) rmw_zenoh_cpp::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(), + new (wait_set->data) rmw_zenoh_cpp::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); - }); + }); - static_cast(wait_set->data)->context = context; + static_cast(wait_set->data)->context = + context; destruct_rmw_wait_set_data.cancel(); free_wait_set_data.cancel(); @@ -3364,20 +3216,18 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) //============================================================================== /// Destroy a wait set. -rmw_ret_t -rmw_destroy_wait_set(rmw_wait_set_t * wait_set) -{ +rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t *wait_set) { 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_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(wait_set, + wait_set->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - auto wait_set_data = static_cast(wait_set->data); + auto wait_set_data = + static_cast(wait_set->data); - rcutils_allocator_t * allocator = &wait_set_data->context->options.allocator; + rcutils_allocator_t *allocator = &wait_set_data->context->options.allocator; wait_set_data->~rmw_wait_set_data_t(); allocator->deallocate(wait_set_data, allocator->state); @@ -3387,21 +3237,18 @@ rmw_destroy_wait_set(rmw_wait_set_t * wait_set) return RMW_RET_OK; } -namespace -{ -bool -check_and_attach_condition( - const rmw_subscriptions_t * const subscriptions, - const rmw_guard_conditions_t * const guard_conditions, - const rmw_services_t * const services, - const rmw_clients_t * const clients, - const rmw_events_t * const events, - rmw_zenoh_cpp::rmw_wait_set_data_t * wait_set_data) -{ +namespace { +bool check_and_attach_condition( + const rmw_subscriptions_t *const subscriptions, + const rmw_guard_conditions_t *const guard_conditions, + const rmw_services_t *const services, const rmw_clients_t *const clients, + const rmw_events_t *const events, + rmw_zenoh_cpp::rmw_wait_set_data_t *wait_set_data) { if (guard_conditions) { for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - rmw_zenoh_cpp::GuardCondition * gc = - static_cast(guard_conditions->guard_conditions[i]); + rmw_zenoh_cpp::GuardCondition *gc = + static_cast( + guard_conditions->guard_conditions[i]); if (gc == nullptr) { continue; } @@ -3415,20 +3262,23 @@ check_and_attach_condition( for (size_t i = 0; i < events->event_count; ++i) { auto event = static_cast(events->events[i]); rmw_zenoh_cpp::rmw_zenoh_event_type_t zenoh_event_type = - rmw_zenoh_cpp::zenoh_event_from_rmw_event(event->event_type); + rmw_zenoh_cpp::zenoh_event_from_rmw_event(event->event_type); if (zenoh_event_type == rmw_zenoh_cpp::ZENOH_EVENT_INVALID) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "has_triggered_condition() called with unknown event %u. Report this bug.", - event->event_type); + "has_triggered_condition() called with unknown event %u. Report " + "this bug.", + event->event_type); continue; } - auto event_data = static_cast(event->data); + auto event_data = + static_cast(event->data); if (event_data == nullptr) { continue; } - if (event_data->queue_has_data_and_attach_condition_if_not(zenoh_event_type, wait_set_data)) { + if (event_data->queue_has_data_and_attach_condition_if_not( + zenoh_event_type, wait_set_data)) { return true; } } @@ -3436,8 +3286,8 @@ check_and_attach_condition( if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - auto sub_data = - static_cast(subscriptions->subscribers[i]); + auto sub_data = static_cast( + subscriptions->subscribers[i]); if (sub_data == nullptr) { continue; } @@ -3449,11 +3299,13 @@ check_and_attach_condition( if (services) { for (size_t i = 0; i < services->service_count; ++i) { - auto serv_data = static_cast(services->services[i]); + auto serv_data = static_cast( + services->services[i]); if (serv_data == nullptr) { continue; } - if (serv_data->queue_has_data_and_attach_condition_if_not(wait_set_data)) { + if (serv_data->queue_has_data_and_attach_condition_if_not( + wait_set_data)) { return true; } } @@ -3461,12 +3313,13 @@ check_and_attach_condition( if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { - rmw_zenoh_cpp::rmw_client_data_t * client_data = - static_cast(clients->clients[i]); + rmw_zenoh_cpp::rmw_client_data_t *client_data = + static_cast(clients->clients[i]); if (client_data == nullptr) { continue; } - if (client_data->queue_has_data_and_attach_condition_if_not(wait_set_data)) { + if (client_data->queue_has_data_and_attach_condition_if_not( + wait_set_data)) { return true; } } @@ -3474,90 +3327,90 @@ check_and_attach_condition( return false; } -} // namespace +} // namespace //============================================================================== /// 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) -{ - 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_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - - 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); +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) { - // rmw_wait should return *all* entities that have data available, and let the caller decide - // how to handle them. + 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_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + 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); + + // rmw_wait should return *all* entities that have data available, and let the + // caller decide how to handle them. // - // If there is no data currently available in any of the entities we were told to wait on, we - // we attach a context-global condition variable to each entity, calculate a timeout based on - // wait_timeout, and then sleep on the condition variable. If any of the entities has an event - // during that time, it will wake up from that sleep. + // If there is no data currently available in any of the entities we were told + // to wait on, we we attach a context-global condition variable to each + // entity, calculate a timeout based on wait_timeout, and then sleep on the + // condition variable. If any of the entities has an event during that time, + // it will wake up from that sleep. // - // If there is data currently available in one or more of the entities, then we'll skip attaching - // the condition variable, and skip the sleep, and instead just go to the last part. + // If there is data currently available in one or more of the entities, then + // we'll skip attaching the condition variable, and skip the sleep, and + // instead just go to the last part. // - // In the last part, we check every entity and see if there are conditions that make it ready. - // If that entity is not ready, then we set the pointer to it to nullptr in the wait set, which - // signals to the upper layers that it isn't ready. If something is ready, then we leave it as - // a valid pointer. - - bool skip_wait = check_and_attach_condition( - subscriptions, guard_conditions, services, clients, events, wait_set_data); + // In the last part, we check every entity and see if there are conditions + // that make it ready. If that entity is not ready, then we set the pointer to + // it to nullptr in the wait set, which signals to the upper layers that it + // isn't ready. If something is ready, then we leave it as a valid pointer. + + bool skip_wait = + check_and_attach_condition(subscriptions, guard_conditions, services, + clients, events, wait_set_data); if (!skip_wait) { 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 as 0 it means "never wait", and if it is anything else wait - // for that amount of time. + // "wait forever", if it specified as 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, [wait_set_data]() { - return wait_set_data->triggered; - }); + lock, [wait_set_data]() { return wait_set_data->triggered; }); } 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)), - [wait_set_data]() {return wait_set_data->triggered;}); + lock, + std::chrono::nanoseconds(wait_timeout->nsec + + RCUTILS_S_TO_NS(wait_timeout->sec)), + [wait_set_data]() { return wait_set_data->triggered; }); } } - // It is important to reset this here while still holding the lock, otherwise every subsequent - // call to rmw_wait() will be immediately ready. We could handle this another way by making - // "triggered" a stack variable in this function and "attaching" it during - // "check_and_attach_condition", but that isn't clearly better so leaving this. + // It is important to reset this here while still holding the lock, + // otherwise every subsequent call to rmw_wait() will be immediately ready. + // We could handle this another way by making "triggered" a stack variable + // in this function and "attaching" it during "check_and_attach_condition", + // but that isn't clearly better so leaving this. wait_set_data->triggered = false; } bool wait_result = false; - // According to the documentation for rmw_wait in rmw.h, entries in the various arrays that have - // *not* been triggered should be set to NULL + // According to the documentation for rmw_wait in rmw.h, entries in the + // various arrays that have *not* been triggered should be set to NULL if (guard_conditions) { for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - rmw_zenoh_cpp::GuardCondition * gc = - static_cast(guard_conditions->guard_conditions[i]); + rmw_zenoh_cpp::GuardCondition *gc = + static_cast( + guard_conditions->guard_conditions[i]); if (gc == nullptr) { continue; } if (!gc->detach_condition_and_is_trigger_set()) { - // Setting to nullptr lets rcl know that this guard condition is not ready + // Setting to nullptr lets rcl know that this guard condition is not + // ready guard_conditions->guard_conditions[i] = nullptr; } else { wait_result = true; @@ -3568,18 +3421,20 @@ rmw_wait( if (events) { for (size_t i = 0; i < events->event_count; ++i) { auto event = static_cast(events->events[i]); - auto event_data = static_cast(event->data); + auto event_data = + static_cast(event->data); if (event_data == nullptr) { continue; } rmw_zenoh_cpp::rmw_zenoh_event_type_t zenoh_event_type = - rmw_zenoh_cpp::zenoh_event_from_rmw_event(event->event_type); + rmw_zenoh_cpp::zenoh_event_from_rmw_event(event->event_type); if (zenoh_event_type == rmw_zenoh_cpp::ZENOH_EVENT_INVALID) { continue; } - if (event_data->detach_condition_and_event_queue_is_empty(zenoh_event_type)) { + if (event_data->detach_condition_and_event_queue_is_empty( + zenoh_event_type)) { // Setting to nullptr lets rcl know that this subscription is not ready events->events[i] = nullptr; } else { @@ -3590,8 +3445,8 @@ rmw_wait( if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - auto sub_data = - static_cast(subscriptions->subscribers[i]); + auto sub_data = static_cast( + subscriptions->subscribers[i]); if (sub_data == nullptr) { continue; } @@ -3607,7 +3462,8 @@ rmw_wait( if (services) { for (size_t i = 0; i < services->service_count; ++i) { - auto serv_data = static_cast(services->services[i]); + auto serv_data = static_cast( + services->services[i]); if (serv_data == nullptr) { continue; } @@ -3623,8 +3479,8 @@ rmw_wait( if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { - rmw_zenoh_cpp::rmw_client_data_t * client_data = - static_cast(clients->clients[i]); + rmw_zenoh_cpp::rmw_client_data_t *client_data = + static_cast(clients->clients[i]); if (client_data == nullptr) { continue; } @@ -3643,34 +3499,29 @@ rmw_wait( //============================================================================== /// 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) -{ +rmw_ret_t rmw_get_node_names(const rmw_node_t *node, + rcutils_string_array_t *node_names, + rcutils_string_array_t *node_namespaces) { + 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; + 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); + node_names, node_namespaces, nullptr, allocator); } //============================================================================== /// Return the name, namespace, 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) -{ +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) { + 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); @@ -3678,36 +3529,33 @@ rmw_get_node_names_with_enclaves( 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; + 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); + node_names, node_namespaces, enclaves, allocator); } //============================================================================== /// 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) -{ +rmw_ret_t rmw_count_publishers(const rmw_node_t *node, const char *topic_name, + size_t *count) { RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT); int validation_result = RMW_TOPIC_VALID; - rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + rmw_ret_t ret = + rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); if (RMW_RET_OK != ret) { return ret; } 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("topic_name argument is invalid: %s", reason); + const char *reason = + rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("topic_name argument is invalid: %s", + reason); return RMW_RET_INVALID_ARGUMENT; } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); @@ -3717,57 +3565,52 @@ rmw_count_publishers( //============================================================================== /// 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) -{ +rmw_ret_t rmw_count_subscribers(const rmw_node_t *node, const char *topic_name, + size_t *count) { RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT); int validation_result = RMW_TOPIC_VALID; - rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + rmw_ret_t ret = + rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); if (RMW_RET_OK != ret) { return ret; } 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("topic_name argument is invalid: %s", reason); + const char *reason = + rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("topic_name argument is invalid: %s", + reason); return RMW_RET_INVALID_ARGUMENT; } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->count_subscriptions(topic_name, count); + return node->context->impl->graph_cache->count_subscriptions(topic_name, + count); } //============================================================================== /// 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) -{ +rmw_ret_t rmw_count_clients(const rmw_node_t *node, const char *service_name, + size_t *count) { RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); int validation_result = RMW_TOPIC_VALID; - rmw_ret_t ret = rmw_validate_full_topic_name(service_name, &validation_result, nullptr); + rmw_ret_t ret = + rmw_validate_full_topic_name(service_name, &validation_result, nullptr); if (RMW_RET_OK != ret) { return ret; } 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("topic_name argument is invalid: %s", reason); + const char *reason = + rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("topic_name argument is invalid: %s", + reason); return RMW_RET_INVALID_ARGUMENT; } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); @@ -3777,27 +3620,24 @@ rmw_count_clients( //============================================================================== /// 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) -{ +rmw_ret_t rmw_count_services(const rmw_node_t *node, const char *service_name, + size_t *count) { RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); int validation_result = RMW_TOPIC_VALID; - rmw_ret_t ret = rmw_validate_full_topic_name(service_name, &validation_result, nullptr); + rmw_ret_t ret = + rmw_validate_full_topic_name(service_name, &validation_result, nullptr); if (RMW_RET_OK != ret) { return ret; } 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("topic_name argument is invalid: %s", reason); + const char *reason = + rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("topic_name argument is invalid: %s", + reason); return RMW_RET_INVALID_ARGUMENT; } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); @@ -3807,14 +3647,13 @@ rmw_count_services( //============================================================================== /// 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) -{ +rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t *publisher, + rmw_gid_t *gid) { RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = - static_cast(publisher->data); + rmw_zenoh_cpp::rmw_publisher_data_t *pub_data = + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; @@ -3825,14 +3664,12 @@ rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) //============================================================================== /// 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) -{ +rmw_ret_t rmw_get_gid_for_client(const rmw_client_t *client, rmw_gid_t *gid) { RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_client_data_t * client_data = - static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t *client_data = + static_cast(client->data); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; memcpy(gid->data, client_data->client_gid, RMW_GID_STORAGE_SIZE); @@ -3842,21 +3679,16 @@ rmw_get_gid_for_client(const rmw_client_t * client, rmw_gid_t * gid) //============================================================================== /// 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) -{ +rmw_ret_t rmw_compare_gids_equal(const rmw_gid_t *gid1, const rmw_gid_t *gid2, + bool *result) { RMW_CHECK_ARGUMENT_FOR_NULL(gid1, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - gid1, - gid1->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(gid1, gid1->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(gid2, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - gid2, - gid2->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(gid2, gid2->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(result, RMW_RET_INVALID_ARGUMENT); *result = memcmp(gid1->data, gid2->data, RMW_GID_STORAGE_SIZE) == 0; @@ -3866,27 +3698,23 @@ rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * re //============================================================================== /// 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) -{ +rmw_ret_t rmw_service_server_is_available(const rmw_node_t *node, + const rmw_client_t *client, + bool *is_available) { RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - rmw_zenoh_cpp::rmw_zenoh_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH(node, node->implementation_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(client->data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(is_available, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_client_data_t * client_data = - static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t *client_data = + static_cast(client->data); if (client_data == nullptr) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Unable to retreive client_data from client for service %s", client->service_name); + "Unable to retreive client_data from client for service %s", + client->service_name); return RMW_RET_INVALID_ARGUMENT; } @@ -3896,39 +3724,36 @@ rmw_service_server_is_available( service_type = service_type.substr(0, suffix_substring_position); } else { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unexpected type %s for client %s. Report this bug", - service_type.c_str(), client->service_name); + "rmw_zenoh_cpp", "Unexpected type %s for client %s. Report this bug", + service_type.c_str(), client->service_name); return RMW_RET_INVALID_ARGUMENT; } return node->context->impl->graph_cache->service_server_is_available( - client->service_name, service_type.c_str(), is_available); + client->service_name, service_type.c_str(), is_available); } //============================================================================== /// Set the current log severity -rmw_ret_t -rmw_set_log_severity(rmw_log_severity_t severity) -{ +rmw_ret_t rmw_set_log_severity(rmw_log_severity_t severity) { switch (severity) { - case RMW_LOG_SEVERITY_DEBUG: - rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_DEBUG); - break; - case RMW_LOG_SEVERITY_INFO: - rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_INFO); - break; - case RMW_LOG_SEVERITY_WARN: - rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_WARN); - break; - case RMW_LOG_SEVERITY_ERROR: - rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_ERROR); - break; - case RMW_LOG_SEVERITY_FATAL: - rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_FATAL); - break; - default: - return RMW_RET_UNSUPPORTED; + case RMW_LOG_SEVERITY_DEBUG: + rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_DEBUG); + break; + case RMW_LOG_SEVERITY_INFO: + rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_INFO); + break; + case RMW_LOG_SEVERITY_WARN: + rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_WARN); + break; + case RMW_LOG_SEVERITY_ERROR: + rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_ERROR); + break; + case RMW_LOG_SEVERITY_FATAL: + rmw_zenoh_cpp::Logger::get().set_log_level(RCUTILS_LOG_SEVERITY_FATAL); + break; + default: + return RMW_RET_UNSUPPORTED; } return RMW_RET_OK; } @@ -3936,51 +3761,40 @@ rmw_set_log_severity(rmw_log_severity_t severity) //============================================================================== /// 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) -{ +rmw_subscription_set_on_new_message_callback(rmw_subscription_t *subscription, + rmw_event_callback_t callback, + const void *user_data) { RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_subscription_data_t * sub_data = - static_cast(subscription->data); + rmw_zenoh_cpp::rmw_subscription_data_t *sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); - sub_data->data_callback_mgr.set_callback( - user_data, callback); + sub_data->data_callback_mgr.set_callback(user_data, callback); return RMW_RET_OK; } //============================================================================== /// 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) -{ +rmw_ret_t rmw_service_set_on_new_request_callback(rmw_service_t *service, + rmw_event_callback_t callback, + const void *user_data) { RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_service_data_t * service_data = - static_cast(service->data); + rmw_zenoh_cpp::rmw_service_data_t *service_data = + static_cast(service->data); RMW_CHECK_ARGUMENT_FOR_NULL(service_data, RMW_RET_INVALID_ARGUMENT); - service_data->data_callback_mgr.set_callback( - user_data, callback); + service_data->data_callback_mgr.set_callback(user_data, callback); return RMW_RET_OK; } //============================================================================== /// 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) -{ +rmw_ret_t rmw_client_set_on_new_response_callback(rmw_client_t *client, + rmw_event_callback_t callback, + const void *user_data) { RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_client_data_t * client_data = - static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t *client_data = + static_cast(client->data); RMW_CHECK_ARGUMENT_FOR_NULL(client_data, RMW_RET_INVALID_ARGUMENT); - client_data->data_callback_mgr.set_callback( - user_data, callback); + client_data->data_callback_mgr.set_callback(user_data, callback); return RMW_RET_OK; } -} // extern "C" +} // extern "C" diff --git a/rmw_zenoh_cpp/src/zenohd/main.cpp b/rmw_zenoh_cpp/src/zenohd/main.cpp index fe6241be..7e90e91a 100644 --- a/rmw_zenoh_cpp/src/zenohd/main.cpp +++ b/rmw_zenoh_cpp/src/zenohd/main.cpp @@ -194,8 +194,8 @@ int main(int argc, char ** argv) return 1; } - z_owned_session_t session = z_open(z_move(config)); - if (!z_check(session)) { + z_owned_session_t session; + if (z_open(&session, z_move(config))) { printf("Unable to open router session!\n"); return 1; } From 2af94a92378b23c2e80a073cad7eb274839653e5 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 18 Jul 2024 18:08:27 +0800 Subject: [PATCH 06/41] fix: memory leak --- .../src/detail/attachment_helpers.cpp | 2 ++ rmw_zenoh_cpp/src/rmw_init.cpp | 6 +++++ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 22 ++++++++++++++++++- 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index ce94663b..7cd76973 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -157,6 +157,8 @@ int64_t get_int64_from_attachment(const z_loaned_bytes_t *const attachment, return -1; } + z_drop(z_move(val)); + return num; } diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 90ede057..d39210f1 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" @@ -185,6 +186,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { // Initialize the zenoh session. z_open(&context->impl->session, z_move(config)); + + if (!z_session_check(&context->impl->session)) { RMW_SET_ERROR_MSG("Error setting up zenoh session"); return RMW_RET_ERROR; @@ -342,6 +345,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { } z_drop(z_move(reply)); + z_drop(z_move(handler)); // TODO(Yadunund): Switch this to a liveliness subscriptions once the API is // available. @@ -382,6 +386,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { return RMW_RET_ERROR; } + z_drop(z_move(keyexpr)); + undeclare_z_sub.cancel(); close_session.cancel(); destruct_guard_condition_data.cancel(); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 0b69771c..b941297b 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -329,6 +329,8 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, node->context = context; node->data = node_data; + z_drop(z_move(keyexpr)); + free_token.cancel(); free_node_data.cancel(); destruct_node_data.cancel(); @@ -682,6 +684,9 @@ rmw_publisher_t *rmw_create_publisher( return nullptr; } + z_drop(z_move(keyexpr)); + z_drop(z_move(liveliness_keyexpr)); + free_token.cancel(); undeclare_z_publisher_cache.cancel(); undeclare_z_publisher.cancel(); @@ -922,7 +927,12 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // allocator->deallocate(msg_bytes, allocator->state); // } // }); - // + auto free_msg_bytes = + rcpputils::make_scope_exit([&msg_bytes, allocator]() { + if (msg_bytes) { + allocator->deallocate(msg_bytes, allocator->state); + } + }); // // Get memory from SHM buffer if available. // if (publisher_data->context->impl->shm_manager.has_value() && // zc_shm_manager_check( @@ -1713,6 +1723,7 @@ rmw_ret_t __rmw_take_one(rmw_zenoh_cpp::rmw_subscription_data_t *sub_data, } *taken = true; + z_drop(z_move(slice)); return RMW_RET_OK; } @@ -1905,6 +1916,7 @@ rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, memcpy(serialized_message->buffer, z_slice_data(z_loan(payload)), z_slice_len(z_loan(payload))); + z_drop(z_move(payload)); *taken = true; if (message_info != nullptr) { @@ -2242,6 +2254,8 @@ rmw_client_t *rmw_create_client( return nullptr; } + z_drop(z_move(keyexpr)); + rmw_client->data = client_data; free_token.cancel(); @@ -2498,6 +2512,7 @@ rmw_ret_t rmw_take_response(const rmw_client_t *client, auto now_ns = std::chrono::duration_cast(now); request_header->received_timestamp = now_ns.count(); + z_drop(z_move(payload)); *taken = true; return RMW_RET_OK; @@ -2810,6 +2825,8 @@ rmw_service_t *rmw_create_service( return nullptr; } + z_drop(z_move(keyexpr)); + rmw_service->data = service_data; free_rmw_service.cancel(); @@ -2971,6 +2988,7 @@ rmw_ret_t rmw_take_request(const rmw_service_t *service, return RMW_RET_ERROR; } + z_drop(z_move(payload)); *taken = true; return RMW_RET_OK; @@ -3053,6 +3071,8 @@ rmw_ret_t rmw_send_response(const rmw_service_t *service, z_query_reply(loaned_query, z_loan(service_data->keyexpr), z_move(payload), &options); + z_drop(z_move(payload)); + return RMW_RET_OK; } From 3ee8cd3f0be3159ea494536c42213ffe7fcf1f32 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 25 Jul 2024 17:57:30 +0800 Subject: [PATCH 07/41] wip --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index b941297b..dc682f58 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -192,6 +192,9 @@ bool rmw_feature_supported(rmw_feature_t feature) { /// 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_) { + + RMW_ZENOH_LOG_ERROR_NAMED("DBG", "rmw_create_node"); + RMW_CHECK_ARGUMENT_FOR_NULL(context, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH(context, context->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, @@ -312,11 +315,7 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, "Failed to declare liveness token."); return nullptr; } - node_data->token = zc_liveliness_declare_token( - z_loan(context->impl->session), - z_keyexpr(node_data->entity->keyexpr().c_str()), - NULL - ); + auto free_token = rcpputils::make_scope_exit( [node_data]() { z_drop(z_move(node_data->token)); }); if (!z_check(node_data->token)) { From e5f5b58f2943a57bab5157697241d26705e9f85f Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Fri, 26 Jul 2024 15:00:23 +0800 Subject: [PATCH 08/41] fix: z_error_t -> z_result_t --- rmw_zenoh_cpp/src/detail/attachment_helpers.cpp | 2 +- rmw_zenoh_cpp/src/detail/attachment_helpers.hpp | 2 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 7cd76973..3571b575 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -51,7 +51,7 @@ bool create_attachment_iter(z_owned_bytes_t *kv_pair, void *context) { return true; } -z_error_t attachement_data_t::serialize_to_zbytes(z_owned_bytes_t *attachment) { +z_result_t attachement_data_t::serialize_to_zbytes(z_owned_bytes_t *attachment) { attachement_context_t context = attachement_context_t(this); return z_bytes_serialize_from_iter(attachment, create_attachment_iter, (void *)&context); diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp index bcc15e92..7e6ee737 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -35,7 +35,7 @@ class attachement_data_t final { source_timestamp = _source_timestamp; memcpy(source_gid, _source_gid, RMW_GID_STORAGE_SIZE); } - z_error_t serialize_to_zbytes(z_owned_bytes_t *); + z_result_t serialize_to_zbytes(z_owned_bytes_t *); }; class attachement_context_t final { diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index dc682f58..914f64d2 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -294,11 +294,11 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, } z_owned_keyexpr_t keyexpr; - z_error_t z_ret = + z_result_t z_ret = z_keyexpr_from_str(&keyexpr, node_data->entity->keyexpr().c_str()); // WARN(yuyuan): z_view_keyexpr_t would fail // z_view_keyexpr_t keyexpr; - // z_error_t z_ret = + // z_result_t z_ret = // z_view_keyexpr_from_str(&keyexpr, // node_data->entity->keyexpr().c_str()); if (z_ret) { @@ -668,7 +668,7 @@ rmw_publisher_t *rmw_create_publisher( // z_view_keyexpr_t liveliness_keyexpr; // z_view_keyexpr_from_str(&liveliness_keyexpr, // publisher_data->entity->keyexpr().c_str()); - z_error_t z_ret = zc_liveliness_declare_token( + z_result_t z_ret = zc_liveliness_declare_token( &publisher_data->token, z_loan(node->context->impl->session), z_loan(liveliness_keyexpr), NULL); auto free_token = rcpputils::make_scope_exit([publisher_data]() { @@ -2768,7 +2768,7 @@ rmw_service_t *rmw_create_service( z_queryable_options_t qable_options; z_queryable_options_default(&qable_options); qable_options.complete = true; - z_error_t z_ret = z_declare_queryable( + z_result_t z_ret = z_declare_queryable( &service_data->qable, z_loan(context_impl->session), z_loan(service_data->keyexpr), z_move(callback), &qable_options); From 28f2fe863f07a44f955ac75521c8301525943e1d Mon Sep 17 00:00:00 2001 From: Mahmoud Mazouz Date: Wed, 24 Jul 2024 12:10:39 +0200 Subject: [PATCH 09/41] Fix `scouting/*/autoconnect/*` per eclipse-zenoh/zenoh@b31a410 (#3) --- rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 | 4 ++-- rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 index 3ba55dfa..0ffdf2bb 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 @@ -44,7 +44,7 @@ /// 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" }, + autoconnect: { router: [], peer: ["router", "peer"] }, /// Whether or not to listen for scout messages on UDP multicast and reply to them. listen: true, }, @@ -61,7 +61,7 @@ /// 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" }, + autoconnect: { router: [], peer: ["router", "peer"] }, }, }, diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 index 6c0ec556..b111bfdf 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 @@ -47,7 +47,7 @@ /// 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" }, + autoconnect: { router: [], peer: ["router", "peer"] }, /// Whether or not to listen for scout messages on UDP multicast and reply to them. listen: true, }, @@ -64,7 +64,7 @@ /// 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" }, + autoconnect: { router: [], peer: ["router", "peer"] }, }, }, From ecf26fc7ce59abca4d37c05294ab154ebc2202ee Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 30 Jul 2024 15:03:29 +0800 Subject: [PATCH 10/41] chore: checkout the local zenoh-c --- 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 943ebb1d..89d69594 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -24,7 +24,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - https://github.com/eclipse-zenoh/zenoh-c/pull/358 (fix debian packaging issue: https://github.com/jspricke/ros-deb-builder-action/issues/49) # - https://github.com/eclipse-zenoh/zenoh/pull/1150 (fix deadlock issue https://github.com/ros2/rmw_zenoh/issues/182) ament_vendor(zenoh_c_vendor - VCS_URL https://github.com/eclipse-zenoh/zenoh-c + VCS_URL file:///workspace/src/zenoh-c VCS_VERSION dev/1.0.0 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" From b9b7cec4b0ae9001f2676f610818930da58287e2 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 15:32:00 +0800 Subject: [PATCH 11/41] chore: polish z_open --- rmw_zenoh_cpp/src/rmw_init.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index d39210f1..4b7af58c 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -185,13 +185,11 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { // }); // Initialize the zenoh session. - z_open(&context->impl->session, z_move(config)); - - - if (!z_session_check(&context->impl->session)) { + if(z_open(&context->impl->session, z_move(config))) { RMW_SET_ERROR_MSG("Error setting up zenoh session"); return RMW_RET_ERROR; } + auto close_session = rcpputils::make_scope_exit( [context]() { z_close(z_move(context->impl->session)); }); From d9cf6f9efdb368711224d17dedb3dd19bc5b92fa Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 15:34:14 +0800 Subject: [PATCH 12/41] fix: segfault --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 21 +-------------------- 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 914f64d2..da8935d9 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -193,7 +193,6 @@ bool rmw_feature_supported(rmw_feature_t feature) { rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, const char *namespace_) { - RMW_ZENOH_LOG_ERROR_NAMED("DBG", "rmw_create_node"); RMW_CHECK_ARGUMENT_FOR_NULL(context, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH(context, context->implementation_identifier, @@ -308,7 +307,7 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, } z_ret = zc_liveliness_declare_token(&node_data->token, - z_loan(node->context->impl->session), + z_loan(context->impl->session), z_loan(keyexpr), NULL); if (z_ret) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", @@ -860,24 +859,6 @@ create_map_and_set_sequence_num(int64_t sequence_number, return bytes; } - // // DBG - // char gid_str[256]; - // gid_str[0] = '\0'; - // for (int i = 0; i < (int)RMW_GID_STORAGE_SIZE; i++) { - // char buffer[50]; // Temporary buffer to hold each number as a string - // sprintf(buffer, "%d, ", gid[i]); - // strcat(gid_str, buffer); // Concatenate buffer to result - // } - // - // z_view_slice_from_str(&key, "source_gid"); - // z_view_slice_wrap(&val, gid, RMW_GID_STORAGE_SIZE); - // z_slice_map_insert_by_copy(z_loan_mut(map), z_loan(key), z_loan(val)); - // - // free_attachment_map.cancel(); - // - // z_bytes_serialize_from_slice_map_copy(&bytes, z_loan(map)); - // z_drop(z_move(map)); - return bytes; } } // namespace From 71700d7f81d85cfb89e277dbfc17a2f47581ee3d Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 16:01:27 +0800 Subject: [PATCH 13/41] feat: `z_bytes_serialize_from_slice` without copy --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 353 +++++++++----------- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 6 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 90 +++-- 3 files changed, 191 insertions(+), 258 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 3406cf4c..3a66f38c 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include @@ -34,10 +35,8 @@ #include "rmw_data_types.hpp" ///============================================================================= -namespace -{ -size_t hash_gid(const uint8_t * gid) -{ +namespace { +size_t hash_gid(const uint8_t *gid) { std::stringstream hash_str; hash_str << std::hex; size_t i = 0; @@ -48,49 +47,33 @@ size_t hash_gid(const uint8_t * gid) } ///============================================================================= -size_t hash_gid(const rmw_request_id_t & request_id) -{ +size_t hash_gid(const rmw_request_id_t &request_id) { return hash_gid(request_id.writer_guid); } -} // namespace +} // namespace ///============================================================================= -size_t rmw_context_impl_s::get_next_entity_id() -{ - return next_entity_id_++; -} +size_t rmw_context_impl_s::get_next_entity_id() { return next_entity_id_++; } -namespace rmw_zenoh_cpp -{ +namespace rmw_zenoh_cpp { ///============================================================================= -saved_msg_data::saved_msg_data( - z_owned_bytes_t p, - uint64_t recv_ts, - const uint8_t pub_gid[RMW_GID_STORAGE_SIZE], - int64_t seqnum, - int64_t source_ts) -: payload(p), recv_timestamp(recv_ts), sequence_number(seqnum), source_timestamp(source_ts) -{ +saved_msg_data::saved_msg_data(std::vector p, uint64_t recv_ts, + const uint8_t pub_gid[RMW_GID_STORAGE_SIZE], + int64_t seqnum, int64_t source_ts) + : payload(p), recv_timestamp(recv_ts), sequence_number(seqnum), + source_timestamp(source_ts) { memcpy(publisher_gid, pub_gid, RMW_GID_STORAGE_SIZE); } ///============================================================================= -saved_msg_data::~saved_msg_data() -{ - z_drop(z_move(payload)); -} - -///============================================================================= -size_t rmw_publisher_data_t::get_next_sequence_number() -{ +size_t rmw_publisher_data_t::get_next_sequence_number() { std::lock_guard lock(sequence_number_mutex_); return sequence_number_++; } ///============================================================================= bool rmw_subscription_data_t::queue_has_data_and_attach_condition_if_not( - rmw_wait_set_data_t * wait_set_data) -{ + rmw_wait_set_data_t *wait_set_data) { std::lock_guard lock(condition_mutex_); if (!message_queue_.empty()) { return true; @@ -102,8 +85,7 @@ bool rmw_subscription_data_t::queue_has_data_and_attach_condition_if_not( } ///============================================================================= -void rmw_subscription_data_t::notify() -{ +void rmw_subscription_data_t::notify() { std::lock_guard lock(condition_mutex_); if (wait_set_data_ != nullptr) { std::lock_guard wait_set_lock(wait_set_data_->condition_mutex); @@ -113,8 +95,7 @@ void rmw_subscription_data_t::notify() } ///============================================================================= -bool rmw_subscription_data_t::detach_condition_and_queue_is_empty() -{ +bool rmw_subscription_data_t::detach_condition_and_queue_is_empty() { std::lock_guard lock(condition_mutex_); wait_set_data_ = nullptr; @@ -122,16 +103,17 @@ bool rmw_subscription_data_t::detach_condition_and_queue_is_empty() } ///============================================================================= -std::unique_ptr rmw_subscription_data_t::pop_next_message() -{ +std::unique_ptr rmw_subscription_data_t::pop_next_message() { std::lock_guard lock(message_queue_mutex_); if (message_queue_.empty()) { - // This tells rcl that the check for a new message was done, but no messages have come in yet. + // This tells rcl that the check for a new message was done, but no messages + // have come in yet. return nullptr; } - std::unique_ptr msg_data = std::move(message_queue_.front()); + std::unique_ptr msg_data = + std::move(message_queue_.front()); message_queue_.pop_front(); return msg_data; @@ -139,45 +121,43 @@ std::unique_ptr rmw_subscription_data_t::pop_next_message() ///============================================================================= void rmw_subscription_data_t::add_new_message( - std::unique_ptr msg, const std::string & topic_name) -{ + std::unique_ptr msg, const std::string &topic_name) { std::lock_guard lock(message_queue_mutex_); if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL && - message_queue_.size() >= adapted_qos_profile.depth) - { + message_queue_.size() >= adapted_qos_profile.depth) { // Log warning if message is discarded due to hitting the queue depth RMW_ZENOH_LOG_DEBUG_NAMED( - "rmw_zenoh_cpp", - "Message queue depth of %ld reached, discarding oldest message " - "for subscription for %s", - adapted_qos_profile.depth, - topic_name.c_str()); - - // If the adapted_qos_profile.depth is 0, the std::move command below will result - // in UB and the z_drop will segfault. We explicitly set the depth to a minimum of 1 - // in rmw_create_subscription() but to be safe, we only attempt to discard from the - // queue if it is non-empty. + "rmw_zenoh_cpp", + "Message queue depth of %ld reached, discarding oldest message " + "for subscription for %s", + adapted_qos_profile.depth, topic_name.c_str()); + + // If the adapted_qos_profile.depth is 0, the std::move command below will + // result in UB and the z_drop will segfault. We explicitly set the depth to + // a minimum of 1 in rmw_create_subscription() but to be safe, we only + // attempt to discard from the queue if it is non-empty. if (!message_queue_.empty()) { std::unique_ptr old = std::move(message_queue_.front()); message_queue_.pop_front(); } } - // Check for messages lost if the new sequence number is not monotonically increasing. + // Check for messages lost if the new sequence number is not monotonically + // increasing. const size_t gid_hash = hash_gid(msg->publisher_gid); auto last_known_pub_it = last_known_published_msg_.find(gid_hash); if (last_known_pub_it != last_known_published_msg_.end()) { - const int64_t seq_increment = std::abs(msg->sequence_number - last_known_pub_it->second); + const int64_t seq_increment = + std::abs(msg->sequence_number - last_known_pub_it->second); if (seq_increment > 1) { const size_t num_msg_lost = seq_increment - 1; total_messages_lost_ += num_msg_lost; auto event_status = std::make_unique(); event_status->total_count_change = num_msg_lost; event_status->total_count = total_messages_lost_; - events_mgr.add_new_event( - ZENOH_EVENT_MESSAGE_LOST, - std::move(event_status)); + events_mgr.add_new_event(ZENOH_EVENT_MESSAGE_LOST, + std::move(event_status)); } } // Always update the last known sequence number for the publisher @@ -185,15 +165,15 @@ void rmw_subscription_data_t::add_new_message( message_queue_.emplace_back(std::move(msg)); - // Since we added new data, trigger user callback and guard condition if they are available + // Since we added new data, trigger user callback and guard condition if they + // are available data_callback_mgr.trigger_callback(); notify(); } ///============================================================================= bool rmw_service_data_t::queue_has_data_and_attach_condition_if_not( - rmw_wait_set_data_t * wait_set_data) -{ + rmw_wait_set_data_t *wait_set_data) { std::lock_guard lock(condition_mutex_); if (!query_queue_.empty()) { return true; @@ -204,8 +184,7 @@ bool rmw_service_data_t::queue_has_data_and_attach_condition_if_not( } ///============================================================================= -bool rmw_service_data_t::detach_condition_and_queue_is_empty() -{ +bool rmw_service_data_t::detach_condition_and_queue_is_empty() { std::lock_guard lock(condition_mutex_); wait_set_data_ = nullptr; @@ -213,8 +192,7 @@ bool rmw_service_data_t::detach_condition_and_queue_is_empty() } ///============================================================================= -std::unique_ptr rmw_service_data_t::pop_next_query() -{ +std::unique_ptr rmw_service_data_t::pop_next_query() { std::lock_guard lock(query_queue_mutex_); if (query_queue_.empty()) { return nullptr; @@ -227,8 +205,7 @@ std::unique_ptr rmw_service_data_t::pop_next_query() } ///============================================================================= -void rmw_service_data_t::notify() -{ +void rmw_service_data_t::notify() { std::lock_guard lock(condition_mutex_); if (wait_set_data_ != nullptr) { std::lock_guard wait_set_lock(wait_set_data_->condition_mutex); @@ -237,42 +214,39 @@ void rmw_service_data_t::notify() } } - ///============================================================================= -void rmw_service_data_t::add_new_query(std::unique_ptr query) -{ +void rmw_service_data_t::add_new_query(std::unique_ptr query) { std::lock_guard lock(query_queue_mutex_); if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL && - query_queue_.size() >= adapted_qos_profile.depth) - { + query_queue_.size() >= adapted_qos_profile.depth) { // Log warning if message is discarded due to hitting the queue depth z_view_string_t keystr; z_keyexpr_as_view_string(z_loan(this->keyexpr), &keystr); RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Query queue depth of %ld reached, discarding oldest Query " - "for service for %s", - adapted_qos_profile.depth, - z_string_data(z_loan(keystr))); + "rmw_zenoh_cpp", + "Query queue depth of %ld reached, discarding oldest Query " + "for service for %s", + adapted_qos_profile.depth, z_string_data(z_loan(keystr))); query_queue_.pop_front(); } query_queue_.emplace_back(std::move(query)); - // Since we added new data, trigger user callback and guard condition if they are available + // Since we added new data, trigger user callback and guard condition if they + // are available data_callback_mgr.trigger_callback(); notify(); } ///============================================================================= -bool rmw_service_data_t::add_to_query_map( - const rmw_request_id_t & request_id, std::unique_ptr query) -{ +bool rmw_service_data_t::add_to_query_map(const rmw_request_id_t &request_id, + std::unique_ptr query) { size_t hash = hash_gid(request_id); std::lock_guard lock(sequence_to_query_map_mutex_); - std::unordered_map::iterator it = sequence_to_query_map_.find(hash); + std::unordered_map::iterator it = + sequence_to_query_map_.find(hash); if (it == sequence_to_query_map_.end()) { SequenceToQuery stq; @@ -288,26 +262,28 @@ bool rmw_service_data_t::add_to_query_map( } } - it->second.insert(std::make_pair(request_id.sequence_number, std::move(query))); + it->second.insert( + std::make_pair(request_id.sequence_number, std::move(query))); return true; } ///============================================================================= -std::unique_ptr rmw_service_data_t::take_from_query_map( - const rmw_request_id_t & request_id) -{ +std::unique_ptr +rmw_service_data_t::take_from_query_map(const rmw_request_id_t &request_id) { size_t hash = hash_gid(request_id); std::lock_guard lock(sequence_to_query_map_mutex_); - std::unordered_map::iterator it = sequence_to_query_map_.find(hash); + std::unordered_map::iterator it = + sequence_to_query_map_.find(hash); if (it == sequence_to_query_map_.end()) { return nullptr; } - SequenceToQuery::iterator query_it = it->second.find(request_id.sequence_number); + SequenceToQuery::iterator query_it = + it->second.find(request_id.sequence_number); if (query_it == it->second.end()) { return nullptr; @@ -324,8 +300,7 @@ std::unique_ptr rmw_service_data_t::take_from_query_map( } ///============================================================================= -void rmw_client_data_t::notify() -{ +void rmw_client_data_t::notify() { std::lock_guard lock(condition_mutex_); if (wait_set_data_ != nullptr) { std::lock_guard wait_set_lock(wait_set_data_->condition_mutex); @@ -335,34 +310,31 @@ void rmw_client_data_t::notify() } ///============================================================================= -void rmw_client_data_t::add_new_reply(std::unique_ptr reply) -{ +void rmw_client_data_t::add_new_reply(std::unique_ptr reply) { std::lock_guard lock(reply_queue_mutex_); if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL && - reply_queue_.size() >= adapted_qos_profile.depth) - { + reply_queue_.size() >= adapted_qos_profile.depth) { // Log warning if message is discarded due to hitting the queue depth z_view_string_t keystr; z_keyexpr_as_view_string(z_loan(this->keyexpr), &keystr); RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Reply queue depth of %ld reached, discarding oldest reply " - "for client for %s", - adapted_qos_profile.depth, - z_string_data(z_loan(keystr))); + "rmw_zenoh_cpp", + "Reply queue depth of %ld reached, discarding oldest reply " + "for client for %s", + adapted_qos_profile.depth, z_string_data(z_loan(keystr))); reply_queue_.pop_front(); } reply_queue_.emplace_back(std::move(reply)); - // Since we added new data, trigger user callback and guard condition if they are available + // Since we added new data, trigger user callback and guard condition if they + // are available data_callback_mgr.trigger_callback(); notify(); } ///============================================================================= bool rmw_client_data_t::queue_has_data_and_attach_condition_if_not( - rmw_wait_set_data_t * wait_set_data) -{ + rmw_wait_set_data_t *wait_set_data) { std::lock_guard lock(condition_mutex_); if (!reply_queue_.empty()) { return true; @@ -373,8 +345,7 @@ bool rmw_client_data_t::queue_has_data_and_attach_condition_if_not( } ///============================================================================= -bool rmw_client_data_t::detach_condition_and_queue_is_empty() -{ +bool rmw_client_data_t::detach_condition_and_queue_is_empty() { std::lock_guard lock(condition_mutex_); wait_set_data_ = nullptr; @@ -382,8 +353,7 @@ bool rmw_client_data_t::detach_condition_and_queue_is_empty() } ///============================================================================= -std::unique_ptr rmw_client_data_t::pop_next_reply() -{ +std::unique_ptr rmw_client_data_t::pop_next_reply() { std::lock_guard lock(reply_queue_mutex_); if (reply_queue_.empty()) { @@ -397,19 +367,17 @@ std::unique_ptr rmw_client_data_t::pop_next_reply() } //============================================================================== -// See the comment about the "num_in_flight" class variable in the rmw_client_data_t class -// for the use of this method. -void rmw_client_data_t::increment_in_flight_callbacks() -{ +// See the comment about the "num_in_flight" class variable in the +// rmw_client_data_t class for the use of this method. +void rmw_client_data_t::increment_in_flight_callbacks() { std::lock_guard lock(in_flight_mutex_); num_in_flight_++; } //============================================================================== -// See the comment about the "num_in_flight" class variable in the rmw_client_data_t class -// for the use of this method. -bool rmw_client_data_t::shutdown_and_query_in_flight() -{ +// See the comment about the "num_in_flight" class variable in the +// rmw_client_data_t class for the use of this method. +bool rmw_client_data_t::shutdown_and_query_in_flight() { std::lock_guard lock(in_flight_mutex_); is_shutdown_ = true; @@ -417,111 +385,104 @@ bool rmw_client_data_t::shutdown_and_query_in_flight() } //============================================================================== -// See the comment about the "num_in_flight" class variable in the rmw_client_data_t structure -// for the use of this method. -bool rmw_client_data_t::decrement_queries_in_flight_and_is_shutdown(bool & queries_in_flight) -{ +// See the comment about the "num_in_flight" class variable in the +// rmw_client_data_t structure for the use of this method. +bool rmw_client_data_t::decrement_queries_in_flight_and_is_shutdown( + bool &queries_in_flight) { std::lock_guard lock(in_flight_mutex_); queries_in_flight = --num_in_flight_ > 0; return is_shutdown_; } -bool rmw_client_data_t::is_shutdown() const -{ +bool rmw_client_data_t::is_shutdown() const { std::lock_guard lock(in_flight_mutex_); return is_shutdown_; } //============================================================================== -void sub_data_handler( - const z_loaned_sample_t * sample, - void * data) -{ +void sub_data_handler(const z_loaned_sample_t *sample, void *data) { z_view_string_t keystr; z_keyexpr_as_view_string(z_sample_keyexpr(sample), &keystr); auto sub_data = static_cast(data); if (sub_data == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to obtain rmw_subscription_data_t from data for " - "subscription for %s", - z_string_data(z_loan(keystr)) - ); + "rmw_zenoh_cpp", + "Unable to obtain rmw_subscription_data_t from data for " + "subscription for %s", + z_string_data(z_loan(keystr))); return; } uint8_t pub_gid[RMW_GID_STORAGE_SIZE]; - const z_loaned_bytes_t* attachment = z_sample_attachment(sample); + const z_loaned_bytes_t *attachment = z_sample_attachment(sample); if (!get_gid_from_attachment(attachment, pub_gid)) { // We failed to get the GID from the attachment. While this isn't fatal, // it is unusual and so we should report it. memset(pub_gid, 0, RMW_GID_STORAGE_SIZE); RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to obtain publisher GID from the attachment."); + "rmw_zenoh_cpp", "Unable to obtain publisher GID from the attachment."); } - int64_t sequence_number = get_int64_from_attachment(attachment, "sequence_number"); + int64_t sequence_number = + get_int64_from_attachment(attachment, "sequence_number"); if (sequence_number < 0) { // We failed to get the sequence number from the attachment. While this // isn't fatal, it is unusual and so we should report it. sequence_number = 0; RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Unable to obtain sequence number from the attachment."); + "rmw_zenoh_cpp", + "Unable to obtain sequence number from the attachment."); } - int64_t source_timestamp = get_int64_from_attachment(attachment, "source_timestamp"); + int64_t source_timestamp = + get_int64_from_attachment(attachment, "source_timestamp"); if (source_timestamp < 0) { // We failed to get the source timestamp from the attachment. While this // isn't fatal, it is unusual and so we should report it. source_timestamp = 0; RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Unable to obtain sequence number from the attachment."); + "rmw_zenoh_cpp", + "Unable to obtain sequence number from the attachment."); } - z_owned_bytes_t payload; - z_bytes_clone(&payload, z_sample_payload(sample)); + const z_loaned_bytes_t *payload = z_sample_payload(sample); + z_bytes_reader_t reader = z_bytes_get_reader(payload); + std::vector payload_vec(z_bytes_len(payload)); + z_bytes_reader_read(&reader, payload_vec.data(), z_bytes_len(payload)); + sub_data->add_new_message( - std::make_unique( - payload, - z_timestamp_ntp64_time(z_sample_timestamp(sample)), - pub_gid, sequence_number, source_timestamp), z_string_data(z_loan(keystr))); + std::make_unique( + std::move(payload_vec), z_timestamp_ntp64_time(z_sample_timestamp(sample)), pub_gid, + sequence_number, source_timestamp), + z_string_data(z_loan(keystr))); } ///============================================================================= -ZenohQuery::ZenohQuery(const z_loaned_query_t * query) -{ - z_query_clone(query_, query); +ZenohQuery::ZenohQuery(const z_loaned_query_t *query) { + z_query_clone(query_, query); } ///============================================================================= -ZenohQuery::~ZenohQuery() -{ - z_drop(z_move(*query_)); -} +ZenohQuery::~ZenohQuery() { z_drop(z_move(*query_)); } ///============================================================================= -const z_loaned_query_t * ZenohQuery::get_query() const -{ +const z_loaned_query_t *ZenohQuery::get_query() const { return z_query_loan(query_); } //============================================================================== -void service_data_handler(const z_loaned_query_t * query, void * data) -{ +void service_data_handler(const z_loaned_query_t *query, void *data) { z_view_string_t keystr; z_keyexpr_as_view_string(z_query_keyexpr(query), &keystr); - rmw_service_data_t * service_data = - static_cast(data); + rmw_service_data_t *service_data = static_cast(data); if (service_data == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to obtain rmw_service_data_t from data for " - "service for %s", - z_string_data(z_loan(keystr)) - ); + "rmw_zenoh_cpp", + "Unable to obtain rmw_service_data_t from data for " + "service for %s", + z_string_data(z_loan(keystr))); return; } @@ -529,46 +490,35 @@ void service_data_handler(const z_loaned_query_t * query, void * data) } ///============================================================================= -ZenohReply::ZenohReply(const z_owned_reply_t * reply) -{ - reply_ = *reply; -} +ZenohReply::ZenohReply(const z_owned_reply_t *reply) { reply_ = *reply; } ///============================================================================= -ZenohReply::~ZenohReply() -{ - z_reply_drop(z_move(reply_)); -} +ZenohReply::~ZenohReply() { z_reply_drop(z_move(reply_)); } // TODO(yuyuan): z_reply_ok return a null pointer if not z_reply_is_ok, // so that's remove the additional optional wrapper. ///============================================================================= -const z_loaned_sample_t * ZenohReply::get_sample() const -{ - return z_reply_ok(z_loan(reply_)); +const z_loaned_sample_t *ZenohReply::get_sample() const { + return z_reply_ok(z_loan(reply_)); } ///============================================================================= -size_t rmw_client_data_t::get_next_sequence_number() -{ +size_t rmw_client_data_t::get_next_sequence_number() { std::lock_guard lock(sequence_number_mutex_); return sequence_number_++; } //============================================================================== -void client_data_handler(const z_loaned_reply_t * reply, void * data) -{ +void client_data_handler(const z_loaned_reply_t *reply, void *data) { auto client_data = static_cast(data); if (client_data == nullptr) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to obtain client_data_t " - ); + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", + "Unable to obtain client_data_t "); return; } - // See the comment about the "num_in_flight" class variable in the rmw_client_data_t class for - // why we need to do this. + // See the comment about the "num_in_flight" class variable in the + // rmw_client_data_t class for why we need to do this. if (client_data->is_shutdown()) { return; } @@ -576,18 +526,17 @@ void client_data_handler(const z_loaned_reply_t * reply, void * data) if (!z_reply_is_ok(reply)) { z_view_string_t keystr; z_keyexpr_as_view_string(z_loan(client_data->keyexpr), &keystr); - const z_loaned_reply_err_t* err = z_reply_err(reply); - const z_loaned_bytes_t* err_payload = z_reply_err_payload(err); + const z_loaned_reply_err_t *err = z_reply_err(reply); + const z_loaned_bytes_t *err_payload = z_reply_err_payload(err); // TODO(yuyuan): z_view_string_t? z_owned_string_t err_str; z_bytes_deserialize_into_string(err_payload, &err_str); RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "z_reply_is_ok returned False for keyexpr %s. Reason: %.*s", - z_string_data(z_loan(keystr)), - (int)z_string_len(z_loan(err_str)), - z_string_data(z_loan(err_str))); + "rmw_zenoh_cpp", + "z_reply_is_ok returned False for keyexpr %s. Reason: %.*s", + z_string_data(z_loan(keystr)), (int)z_string_len(z_loan(err_str)), + z_string_data(z_loan(err_str))); z_drop(z_move(err_str)); return; } @@ -595,39 +544,35 @@ void client_data_handler(const z_loaned_reply_t * reply, void * data) z_reply_clone(&owned_reply, reply); if (!z_reply_check(&owned_reply)) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "z_reply_check returned False" - ); + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "z_reply_check returned False"); return; } client_data->add_new_reply(std::make_unique(&owned_reply)); } -void client_data_drop(void * data) -{ +void client_data_drop(void *data) { auto client_data = static_cast(data); if (client_data == nullptr) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to obtain client_data_t " - ); + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", + "Unable to obtain client_data_t "); return; } - // See the comment about the "num_in_flight" class variable in the rmw_client_data_t class for - // why we need to do this. + // See the comment about the "num_in_flight" class variable in the + // rmw_client_data_t class for why we need to do this. bool queries_in_flight = false; - bool is_shutdown = client_data->decrement_queries_in_flight_and_is_shutdown(queries_in_flight); + bool is_shutdown = client_data->decrement_queries_in_flight_and_is_shutdown( + queries_in_flight); if (is_shutdown) { if (!queries_in_flight) { - RMW_TRY_DESTRUCTOR(client_data->~rmw_client_data_t(), rmw_client_data_t, ); + RMW_TRY_DESTRUCTOR(client_data->~rmw_client_data_t(), + rmw_client_data_t, ); client_data->context->options.allocator.deallocate( - client_data, client_data->context->options.allocator.state); + client_data, client_data->context->options.allocator.state); } } } -} // namespace rmw_zenoh_cpp +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index dad46940..7a5be287 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -137,15 +137,15 @@ void sub_data_handler(const z_loaned_sample_t * sample, void * sub_data); struct saved_msg_data { explicit saved_msg_data( - z_owned_bytes_t p, + std::vector p, uint64_t recv_ts, const uint8_t pub_gid[RMW_GID_STORAGE_SIZE], int64_t seqnum, int64_t source_ts); - ~saved_msg_data(); + // ~saved_msg_data(); - z_owned_bytes_t payload; + std::vector payload; uint64_t recv_timestamp; uint8_t publisher_gid[RMW_GID_STORAGE_SIZE]; int64_t sequence_number; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index da8935d9..e41fced9 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -193,7 +193,6 @@ bool rmw_feature_supported(rmw_feature_t feature) { rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, const char *namespace_) { - RMW_CHECK_ARGUMENT_FOR_NULL(context, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH(context, context->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, @@ -257,27 +256,24 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, }); // Put metadata into node->data. - auto node_data = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::rmw_node_data_t), allocator->state)); + auto node_data = + static_cast(allocator->allocate( + sizeof(rmw_zenoh_cpp::rmw_node_data_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - node_data, - "failed to allocate memory for node data", - return nullptr); - auto free_node_data = rcpputils::make_scope_exit( - [node_data, allocator]() { - allocator->deallocate(node_data, allocator->state); - }); - - RMW_TRY_PLACEMENT_NEW( - node_data, node_data, return nullptr, - rmw_zenoh_cpp::rmw_node_data_t); - auto destruct_node_data = rcpputils::make_scope_exit( - [node_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - node_data->~rmw_node_data_t(), rmw_zenoh_cpp::rmw_node_data_t); - }); - - // Initialize liveliness token for the node to advertise that a new node is in town. + node_data, "failed to allocate memory for node data", return nullptr); + auto free_node_data = rcpputils::make_scope_exit([node_data, allocator]() { + allocator->deallocate(node_data, allocator->state); + }); + + RMW_TRY_PLACEMENT_NEW(node_data, node_data, return nullptr, + rmw_zenoh_cpp::rmw_node_data_t); + auto destruct_node_data = rcpputils::make_scope_exit([node_data]() { + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(node_data->~rmw_node_data_t(), + rmw_zenoh_cpp::rmw_node_data_t); + }); + + // Initialize liveliness token for the node to advertise that a new node is in + // town. node_data->id = context->impl->get_next_entity_id(); node_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(z_loan(context->impl->session)), std::to_string(node_data->id), @@ -306,9 +302,8 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, return nullptr; } - z_ret = zc_liveliness_declare_token(&node_data->token, - z_loan(context->impl->session), - z_loan(keyexpr), NULL); + z_ret = zc_liveliness_declare_token( + &node_data->token, z_loan(context->impl->session), z_loan(keyexpr), NULL); if (z_ret) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Failed to declare liveness token."); @@ -350,12 +345,12 @@ rmw_ret_t rmw_destroy_node(rmw_node_t *node) { rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - rcutils_allocator_t * allocator = &node->context->options.allocator; + rcutils_allocator_t *allocator = &node->context->options.allocator; - // Undeclare liveliness token for the node to advertise that the node has ridden - // off into the sunset. - rmw_zenoh_cpp::rmw_node_data_t * node_data = - static_cast(node->data); + // Undeclare liveliness token for the node to advertise that the node has + // ridden off into the sunset. + rmw_zenoh_cpp::rmw_node_data_t *node_data = + static_cast(node->data); if (node_data != nullptr) { zc_liveliness_undeclare_token(z_move(node_data->token)); RMW_TRY_DESTRUCTOR(node_data->~rmw_node_data_t(), rmw_node_data_t, ); @@ -907,12 +902,11 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // allocator->deallocate(msg_bytes, allocator->state); // } // }); - auto free_msg_bytes = - rcpputils::make_scope_exit([&msg_bytes, allocator]() { - if (msg_bytes) { - allocator->deallocate(msg_bytes, allocator->state); - } - }); + auto free_msg_bytes = rcpputils::make_scope_exit([&msg_bytes, allocator]() { + if (msg_bytes) { + allocator->deallocate(msg_bytes, allocator->state); + } + }); // // Get memory from SHM buffer if available. // if (publisher_data->context->impl->shm_manager.has_value() && // zc_shm_manager_check( @@ -994,7 +988,7 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // } z_owned_bytes_t payload; - z_bytes_serialize_from_slice_copy( + z_bytes_serialize_from_slice( &payload, reinterpret_cast(msg_bytes), data_length); ret = z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options); if (ret) { @@ -1672,14 +1666,10 @@ rmw_ret_t __rmw_take_one(rmw_zenoh_cpp::rmw_subscription_data_t *sub_data, return RMW_RET_OK; } - z_owned_slice_t slice; - z_bytes_deserialize_into_slice(z_loan(msg_data->payload), &slice); - // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast( - const_cast(z_slice_data(z_loan(slice)))), - z_slice_len(z_loan(slice))); + reinterpret_cast(msg_data->payload.data()), +msg_data->payload.size()); // Object that serializes the data rmw_zenoh_cpp::Cdr deser(fastbuffer); @@ -1703,7 +1693,6 @@ rmw_ret_t __rmw_take_one(rmw_zenoh_cpp::rmw_subscription_data_t *sub_data, } *taken = true; - z_drop(z_move(slice)); return RMW_RET_OK; } @@ -1882,21 +1871,20 @@ rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, return RMW_RET_OK; } - z_owned_slice_t payload; - z_bytes_deserialize_into_slice(z_loan(msg_data->payload), &payload); + const uint8_t* payload = msg_data->payload.data(); + const size_t payload_len = msg_data->payload.size(); - if (serialized_message->buffer_capacity < z_slice_len(z_loan(payload))) { + if (serialized_message->buffer_capacity < payload_len) { rmw_ret_t ret = rmw_serialized_message_resize(serialized_message, - z_slice_len(z_loan(payload))); + payload_len); if (ret != RMW_RET_OK) { return ret; // Error message already set } } - serialized_message->buffer_length = z_slice_len(z_loan(payload)); - memcpy(serialized_message->buffer, z_slice_data(z_loan(payload)), - z_slice_len(z_loan(payload))); + serialized_message->buffer_length = payload_len; + memcpy(serialized_message->buffer, payload, + payload_len); - z_drop(z_move(payload)); *taken = true; if (message_info != nullptr) { From cc8b1d6d8c735e2458e86b9d5f16ef0036b36066 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 16:14:47 +0800 Subject: [PATCH 14/41] chore: switch back to https://github.com/eclipse-zenoh/zenoh-c --- 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 89d69594..943ebb1d 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -24,7 +24,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - https://github.com/eclipse-zenoh/zenoh-c/pull/358 (fix debian packaging issue: https://github.com/jspricke/ros-deb-builder-action/issues/49) # - https://github.com/eclipse-zenoh/zenoh/pull/1150 (fix deadlock issue https://github.com/ros2/rmw_zenoh/issues/182) ament_vendor(zenoh_c_vendor - VCS_URL file:///workspace/src/zenoh-c + VCS_URL https://github.com/eclipse-zenoh/zenoh-c VCS_VERSION dev/1.0.0 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" From d26133220c52f46019f7201bf91689cbbabb2f0f Mon Sep 17 00:00:00 2001 From: Mahmoud Mazouz Date: Fri, 26 Jul 2024 18:53:53 +0200 Subject: [PATCH 15/41] Initialize `query_` member of `ZenohQuery` --- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 7a5be287..ee1adf73 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -218,7 +218,7 @@ class ZenohQuery final const z_loaned_query_t * get_query() const; private: - z_owned_query_t * query_; + z_owned_query_t * query_{nullptr}; }; ///============================================================================= From 15ea13986b04e6b65e01f55db65031ebc1388025 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 18:27:29 +0800 Subject: [PATCH 16/41] refactor: use `z_owned_slice_t` instead --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 14 +++++++++----- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 6 +++--- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 11 ++++++----- 3 files changed, 18 insertions(+), 13 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 3a66f38c..cb66e460 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -57,13 +57,18 @@ size_t rmw_context_impl_s::get_next_entity_id() { return next_entity_id_++; } namespace rmw_zenoh_cpp { ///============================================================================= -saved_msg_data::saved_msg_data(std::vector p, uint64_t recv_ts, +saved_msg_data::saved_msg_data(z_owned_slice_t p, uint64_t recv_ts, const uint8_t pub_gid[RMW_GID_STORAGE_SIZE], int64_t seqnum, int64_t source_ts) : payload(p), recv_timestamp(recv_ts), sequence_number(seqnum), source_timestamp(source_ts) { memcpy(publisher_gid, pub_gid, RMW_GID_STORAGE_SIZE); } +///============================================================================= +saved_msg_data::~saved_msg_data() +{ + z_drop(z_move(payload)); +} ///============================================================================= size_t rmw_publisher_data_t::get_next_sequence_number() { @@ -447,13 +452,12 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) { } const z_loaned_bytes_t *payload = z_sample_payload(sample); - z_bytes_reader_t reader = z_bytes_get_reader(payload); - std::vector payload_vec(z_bytes_len(payload)); - z_bytes_reader_read(&reader, payload_vec.data(), z_bytes_len(payload)); + z_owned_slice_t slice; + z_bytes_deserialize_into_slice(payload, &slice); sub_data->add_new_message( std::make_unique( - std::move(payload_vec), z_timestamp_ntp64_time(z_sample_timestamp(sample)), pub_gid, + slice, z_timestamp_ntp64_time(z_sample_timestamp(sample)), pub_gid, sequence_number, source_timestamp), z_string_data(z_loan(keystr))); } diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index ee1adf73..6c092182 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -137,15 +137,15 @@ void sub_data_handler(const z_loaned_sample_t * sample, void * sub_data); struct saved_msg_data { explicit saved_msg_data( - std::vector p, + z_owned_slice_t p, uint64_t recv_ts, const uint8_t pub_gid[RMW_GID_STORAGE_SIZE], int64_t seqnum, int64_t source_ts); - // ~saved_msg_data(); + ~saved_msg_data(); - std::vector payload; + z_owned_slice_t payload; uint64_t recv_timestamp; uint8_t publisher_gid[RMW_GID_STORAGE_SIZE]; int64_t sequence_number; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index e41fced9..2e32ca36 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1666,10 +1666,11 @@ rmw_ret_t __rmw_take_one(rmw_zenoh_cpp::rmw_subscription_data_t *sub_data, return RMW_RET_OK; } + const uint8_t* payload = z_slice_data(z_loan(msg_data->payload)); + const size_t payload_len = z_slice_len(z_loan(msg_data->payload)); + // Object that manages the raw buffer - eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(msg_data->payload.data()), -msg_data->payload.size()); + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(const_cast(payload)), payload_len); // Object that serializes the data rmw_zenoh_cpp::Cdr deser(fastbuffer); @@ -1871,8 +1872,8 @@ rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, return RMW_RET_OK; } - const uint8_t* payload = msg_data->payload.data(); - const size_t payload_len = msg_data->payload.size(); + const uint8_t* payload = z_slice_data(z_loan(msg_data->payload)); + const size_t payload_len = z_slice_len(z_loan(msg_data->payload)); if (serialized_message->buffer_capacity < payload_len) { rmw_ret_t ret = rmw_serialized_message_resize(serialized_message, From ced2c14141a35e242810fdc1aeda333535fb8a9f Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Sat, 3 Aug 2024 00:17:01 +0800 Subject: [PATCH 17/41] chore: adapt the latest change of zenoh-c dev/1.0.0 --- .../src/detail/attachment_helpers.cpp | 19 +++++++++---------- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 8 ++++---- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 3571b575..22b63d65 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -40,20 +40,20 @@ bool create_attachment_iter(z_owned_bytes_t *kv_pair, void *context) { z_bytes_serialize_from_int64(&v, ctx->data->source_timestamp); } else if (ctx->idx == 2) { z_bytes_serialize_from_str(&k, "source_gid"); - z_bytes_serialize_from_slice_copy(&v, ctx->data->source_gid, + z_bytes_serialize_from_buf(&v, ctx->data->source_gid, RMW_GID_STORAGE_SIZE); } else { return false; } - z_bytes_serialize_from_pair(kv_pair, z_move(k), z_move(v)); + z_bytes_from_pair(kv_pair, z_move(k), z_move(v)); ctx->idx += 1; return true; } z_result_t attachement_data_t::serialize_to_zbytes(z_owned_bytes_t *attachment) { attachement_context_t context = attachement_context_t(this); - return z_bytes_serialize_from_iter(attachment, create_attachment_iter, + return z_bytes_from_iter(attachment, create_attachment_iter, (void *)&context); } @@ -72,17 +72,16 @@ bool get_attachment(const z_loaned_bytes_t *const attachment, z_owned_string_t key_string; z_bytes_deserialize_into_string(z_loan(key_), &key_string); - char dbg_info[120]; - sprintf(dbg_info, "Given key: %s, found: %s", key.c_str(), - z_string_data(z_loan(key_string))); - sprintf(dbg_info, "Given key: %s, found: %.*s", key.c_str(), - (int)z_string_len(z_loan(key_string)), - z_string_data(z_loan(key_string))); + // TODO(yuyuan): use strncmp + // const char* key_string_ptr = z_string_data(z_loan(key_string)); + // size_t key_string_len = z_string_len(z_loan(key_string)); + // if (key_string_len == key.length() && strncmp(key_string_ptr, key.c_str(), key.length())) { + // found = true; + // } std::string found_key; found_key.assign(z_string_data(z_loan(key_string)), z_string_len(z_loan(key_string))); if (found_key == key) { - // if (strcmp(z_string_data(z_loan(key_string)), key.c_str()) == 0) { found = true; } diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 2e32ca36..3938b71c 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -988,7 +988,7 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // } z_owned_bytes_t payload; - z_bytes_serialize_from_slice( + z_bytes_serialize_from_buf( &payload, reinterpret_cast(msg_bytes), data_length); ret = z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options); if (ret) { @@ -1107,7 +1107,7 @@ rmw_ret_t rmw_publish_serialized_message( options.attachment = &attachment; z_owned_bytes_t payload; - z_bytes_serialize_from_slice_copy(&payload, serialized_message->buffer, + z_bytes_serialize_from_buf(&payload, serialized_message->buffer, data_length); if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options)) { @@ -2380,7 +2380,7 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, // expression, which optimizes bandwidth. The default is "None", which imples // replies may come in any order and any number. opts.consolidation = z_query_consolidation_latest(); - z_bytes_serialize_from_slice_copy( + z_bytes_serialize_from_buf( opts.payload, reinterpret_cast(request_bytes), data_length); @@ -3035,7 +3035,7 @@ rmw_ret_t rmw_send_response(const rmw_service_t *service, return RMW_RET_ERROR; } z_owned_bytes_t payload; - z_bytes_serialize_from_slice( + z_bytes_serialize_from_buf( &payload, reinterpret_cast(response_bytes), data_length); z_query_reply(loaned_query, z_loan(service_data->keyexpr), z_move(payload), &options); From 55efb74c0a7dd1c7b95ec437766b6d0c837dad3c Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 7 Aug 2024 12:09:29 +0800 Subject: [PATCH 18/41] chore: use `strncmp` to avoid copying --- rmw_zenoh_cpp/src/detail/attachment_helpers.cpp | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 22b63d65..52e55dd9 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -72,16 +72,9 @@ bool get_attachment(const z_loaned_bytes_t *const attachment, z_owned_string_t key_string; z_bytes_deserialize_into_string(z_loan(key_), &key_string); - // TODO(yuyuan): use strncmp - // const char* key_string_ptr = z_string_data(z_loan(key_string)); - // size_t key_string_len = z_string_len(z_loan(key_string)); - // if (key_string_len == key.length() && strncmp(key_string_ptr, key.c_str(), key.length())) { - // found = true; - // } - - std::string found_key; - found_key.assign(z_string_data(z_loan(key_string)), z_string_len(z_loan(key_string))); - if (found_key == key) { + const char* key_string_ptr = z_string_data(z_loan(key_string)); + size_t key_string_len = z_string_len(z_loan(key_string)); + if (key_string_len == key.length() && strncmp(key_string_ptr, key.c_str(), key.length()) == 0) { found = true; } From 8505e340209a36b1c3a4fa9cc82a59a4af8de400 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 8 Aug 2024 16:25:41 +0800 Subject: [PATCH 19/41] refactor: use `z_view_keyexpr_t` to avoid copying --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 3938b71c..f339b09e 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -288,23 +288,17 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, return nullptr; } - z_owned_keyexpr_t keyexpr; - z_result_t z_ret = - z_keyexpr_from_str(&keyexpr, node_data->entity->keyexpr().c_str()); - // WARN(yuyuan): z_view_keyexpr_t would fail - // z_view_keyexpr_t keyexpr; - // z_result_t z_ret = - // z_view_keyexpr_from_str(&keyexpr, - // node_data->entity->keyexpr().c_str()); - if (z_ret) { + std::string keyexpr_str = node_data->entity->keyexpr(); + z_view_keyexpr_t keyexpr; + if (z_view_keyexpr_from_str(&keyexpr, keyexpr_str.c_str())) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to generate keyexpr from the entity string."); return nullptr; } - z_ret = zc_liveliness_declare_token( - &node_data->token, z_loan(context->impl->session), z_loan(keyexpr), NULL); - if (z_ret) { + if (zc_liveliness_declare_token(&node_data->token, + z_loan(context->impl->session), + z_loan(keyexpr), NULL)) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Failed to declare liveness token."); return nullptr; @@ -322,8 +316,6 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, node->context = context; node->data = node_data; - z_drop(z_move(keyexpr)); - free_token.cancel(); free_node_data.cancel(); destruct_node_data.cancel(); From c43caf24687b18826563eaa25091eba5194b98ea Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 8 Aug 2024 16:26:35 +0800 Subject: [PATCH 20/41] feat: implment the SHM feature --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 1 + rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 6 +- rmw_zenoh_cpp/src/rmw_init.cpp | 81 ++++++------ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 133 ++++++++++++-------- 4 files changed, 118 insertions(+), 103 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index cb66e460..d39084ae 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -452,6 +452,7 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) { } const z_loaned_bytes_t *payload = z_sample_payload(sample); + z_owned_slice_t slice; z_bytes_deserialize_into_slice(payload, &slice); diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 6c092182..0b314d35 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -52,10 +52,8 @@ class rmw_context_impl_s final // An owned session. z_owned_session_t session; - // TODO(yuyuan): SHM provider - // An optional SHM manager that is initialized of SHM is enabled in the - // zenoh session config. - // std::optional shm_manager; + // TODO(yuyuan): SHM + std::optional shm_provider; z_owned_subscriber_t graph_subscriber; diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 4b7af58c..4154e2ef 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 #include #include @@ -175,14 +176,13 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { return ret; } - // // TODO(yuyuan): SHM is disabled - // // 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)); - // }); + // TODO(yuyuan): SHM + z_owned_string_t shm_enabled; + zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled); + auto free_shm_= rcpputils::make_scope_exit( + [&shm_enabled]() { + z_drop(z_move(shm_enabled)); + }); // Initialize the zenoh session. if(z_open(&context->impl->session, z_move(config))) { @@ -220,39 +220,30 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { } } - // // TODO(yuyuan): SHM is disabled - // // Initialize the shm manager if shared_memory is enabled in the config. - // if (shm_enabled._cstr != nullptr && - // strcmp(shm_enabled._cstr, "true") == 0) - // { - // char idstr[sizeof(zid.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(zid.id); ++i) { - // snprintf(idstr + 2 * i, max_size_of_each, "%02x", zid.id[i]); - // } - // idstr[sizeof(zid.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 = - // zc_shm_manager_new( - // z_loan(context->impl->session), - // idstr, - // SHM_BUFFER_SIZE_MB * 1024 * 1024); - // if (!context->impl->shm_manager.has_value() || - // !zc_shm_manager_check(&context->impl->shm_manager.value())) - // { - // RMW_SET_ERROR_MSG("Unable to create shm manager."); - // return RMW_RET_ERROR; - // } - // } - // auto free_shm_manager = rcpputils::make_scope_exit( - // [context]() { - // if (context->impl->shm_manager.has_value()) { - // z_drop(z_move(context->impl->shm_manager.value())); - // } - // }); + // TODO(yuyuan): SHM + // Initialize the shm manager if shared_memory is enabled in the config. + if (strncmp(z_string_data(z_loan(shm_enabled)), "true", z_string_len(z_loan(shm_enabled))) == 0) { + printf(">>> SHM is enabled\n"); + + // TODO(yuyuan): determine the default alignment + z_alloc_alignment_t alignment = {5}; + z_owned_memory_layout_t layout; + z_memory_layout_new(&layout, SHM_BUFFER_SIZE_MB * 1024 * 1024, alignment); + + z_owned_shm_provider_t provider; + if (z_posix_shm_provider_new(&provider, z_loan(layout))) { + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a SHM provider."); + return RMW_RET_ERROR; + } + context->impl->shm_provider = std::make_optional(provider); + } + + auto free_shm_provider = rcpputils::make_scope_exit( + [context]() { + if (context->impl->shm_provider.has_value()) { + z_drop(z_move(context->impl->shm_provider.value())); + } + }); // Initialize the guard condition. context->impl->graph_guard_condition = @@ -396,7 +387,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { free_options.cancel(); impl_destructor.cancel(); free_impl.cancel(); - // free_shm_manager.cancel(); + free_shm_provider.cancel(); restore_context.cancel(); return RMW_RET_OK; @@ -413,9 +404,9 @@ rmw_ret_t rmw_shutdown(rmw_context_t *context) { return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); - // if (context->impl->shm_manager.has_value()) { - // z_drop(z_move(context->impl->shm_manager.value())); - // } + if (context->impl->shm_provider.has_value()) { + z_drop(z_move(context->impl->shm_provider.value())); + } // 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 f339b09e..886dd0fa 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -160,6 +160,10 @@ find_service_type_support(const rosidl_service_type_support_t *type_supports) { } // namespace extern "C" { + +// TODO(yuyuan): SHM, make this configurable +#define SHM_BUF_OK_SIZE 2621440 + //============================================================================== /// Get the name of the rmw implementation being used const char *rmw_get_implementation_identifier(void) { @@ -881,24 +885,54 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // To store serialized message byte array. char *msg_bytes = nullptr; - // // TODO(yuyuan): disable SHM - // std::optional shmbuf = std::nullopt; - // auto always_free_shmbuf = rcpputils::make_scope_exit([&shmbuf]() { - // if (shmbuf.has_value()) { - // zc_shmbuf_drop(&shmbuf.value()); - // } - // }); - // auto free_msg_bytes = - // rcpputils::make_scope_exit([&msg_bytes, allocator, &shmbuf]() { - // if (msg_bytes && !shmbuf.has_value()) { - // allocator->deallocate(msg_bytes, allocator->state); - // } - // }); - auto free_msg_bytes = rcpputils::make_scope_exit([&msg_bytes, allocator]() { - if (msg_bytes) { - allocator->deallocate(msg_bytes, allocator->state); + // TODO(yuyuan): SHM + std::optional shmbuf = std::nullopt; + auto always_free_shmbuf = rcpputils::make_scope_exit([&shmbuf]() { + if (shmbuf.has_value()) { + // TODO(yuyuan): do we need to drop z_owned_shm_mut_t? + z_drop(z_move(shmbuf.value())); } }); + auto free_msg_bytes = + rcpputils::make_scope_exit([&msg_bytes, allocator, &shmbuf]() { + if (msg_bytes && !shmbuf.has_value()) { + allocator->deallocate(msg_bytes, allocator->state); + } + }); + + // TODO(yuyuan): SHM + // Get memory from SHM buffer if available. + if (publisher_data->context->impl->shm_provider.has_value()) { + // printf(">>> rmw_publish(), SHM enabled\n"); + + auto provider = publisher_data->context->impl->shm_provider.value(); + z_buf_layout_alloc_result_t alloc; + // TODO(yuyuan): SHM, configure this + z_alloc_alignment_t alignment = {5}; + z_shm_provider_alloc_gc_defrag_blocking( + &alloc, z_loan(provider), + SHM_BUF_OK_SIZE, alignment); + + if (z_check(alloc.buf)) { + shmbuf = std::make_optional(alloc.buf); + msg_bytes = + reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); + } else { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Unexpected failure during SHM buffer allocation."); + return RMW_RET_ERROR; + } + + } else { + // printf(">>> rmw_publish(), SHM disabled\n"); + + // 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); + } + // // Get memory from SHM buffer if available. // if (publisher_data->context->impl->shm_manager.has_value() && // zc_shm_manager_check( @@ -927,11 +961,12 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // RMW_CHECK_FOR_NULL_WITH_MSG(msg_bytes, "bytes for message is null", // return RMW_RET_BAD_ALLOC); // } - // 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); + + // // 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); // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(msg_bytes, max_data_length); @@ -957,7 +992,6 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, auto free_attachment = rcpputils::make_scope_exit( [&attachment]() { z_drop(z_move(attachment)); }); - 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. @@ -965,27 +999,19 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, z_publisher_put_options_default(&options); options.attachment = &attachment; - // // TODO(yuyuan): disable SHM - // if (shmbuf.has_value()) { - // zc_shmbuf_set_length(&shmbuf.value(), data_length); - // zc_owned_payload_t payload = - // zc_shmbuf_into_payload(z_move(shmbuf.value())); 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), - // reinterpret_cast(msg_bytes), - // data_length, &options); - // } - z_owned_bytes_t payload; - z_bytes_serialize_from_buf( - &payload, reinterpret_cast(msg_bytes), data_length); - ret = z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options); - if (ret) { - RMW_SET_ERROR_MSG("unable to publish message"); - return RMW_RET_ERROR; + + // TODO(yuyuan): SHM + if (shmbuf.has_value()) { + z_bytes_serialize_from_shm_mut(&payload, &shmbuf.value()); + z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options); + } else { + z_bytes_serialize_from_buf( + &payload, reinterpret_cast(msg_bytes), data_length); + if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options)) { + RMW_SET_ERROR_MSG("unable to publish message"); + return RMW_RET_ERROR; + } } return RMW_RET_OK; @@ -1099,8 +1125,7 @@ rmw_ret_t rmw_publish_serialized_message( options.attachment = &attachment; z_owned_bytes_t payload; - z_bytes_serialize_from_buf(&payload, serialized_message->buffer, - data_length); + z_bytes_serialize_from_buf(&payload, serialized_message->buffer, data_length); if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options)) { RMW_SET_ERROR_MSG("unable to publish message"); @@ -1658,11 +1683,12 @@ rmw_ret_t __rmw_take_one(rmw_zenoh_cpp::rmw_subscription_data_t *sub_data, return RMW_RET_OK; } - const uint8_t* payload = z_slice_data(z_loan(msg_data->payload)); + const uint8_t *payload = z_slice_data(z_loan(msg_data->payload)); const size_t payload_len = z_slice_len(z_loan(msg_data->payload)); // Object that manages the raw buffer - eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(const_cast(payload)), payload_len); + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(const_cast(payload)), payload_len); // Object that serializes the data rmw_zenoh_cpp::Cdr deser(fastbuffer); @@ -1864,19 +1890,18 @@ rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, return RMW_RET_OK; } - const uint8_t* payload = z_slice_data(z_loan(msg_data->payload)); + const uint8_t *payload = z_slice_data(z_loan(msg_data->payload)); const size_t payload_len = z_slice_len(z_loan(msg_data->payload)); if (serialized_message->buffer_capacity < payload_len) { - rmw_ret_t ret = rmw_serialized_message_resize(serialized_message, - payload_len); + rmw_ret_t ret = + rmw_serialized_message_resize(serialized_message, payload_len); if (ret != RMW_RET_OK) { return ret; // Error message already set } } serialized_message->buffer_length = payload_len; - memcpy(serialized_message->buffer, payload, - payload_len); + memcpy(serialized_message->buffer, payload, payload_len); *taken = true; @@ -2372,9 +2397,9 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, // expression, which optimizes bandwidth. The default is "None", which imples // replies may come in any order and any number. opts.consolidation = z_query_consolidation_latest(); - z_bytes_serialize_from_buf( - opts.payload, reinterpret_cast(request_bytes), - data_length); + z_bytes_serialize_from_buf(opts.payload, + reinterpret_cast(request_bytes), + data_length); z_owned_closure_reply_t callback; z_closure(&callback, rmw_zenoh_cpp::client_data_handler, NULL, client_data); From 686dec18c6e578c09389dc2e3ba3e54d269d9225 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 13 Aug 2024 17:09:22 +0200 Subject: [PATCH 21/41] Replaced ament_vendor with ExternalProject --- zenoh_c_vendor/CMakeLists.txt | 28 +++++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 943ebb1d..0aea2753 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() find_package(ament_cmake REQUIRED) -find_package(ament_cmake_vendor_package REQUIRED) +# find_package(ament_cmake_vendor_package REQUIRED) # Disable default features and enable only the most useful ones. This reduces build time and footprint. # For a complete list of features see: https://github.com/eclipse-zenoh/zenoh/blob/main/zenoh/Cargo.toml @@ -23,11 +23,25 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - https://github.com/eclipse-zenoh/zenoh/pull/1022 (fix empty messages received if payload >btach size) # - https://github.com/eclipse-zenoh/zenoh-c/pull/358 (fix debian packaging issue: https://github.com/jspricke/ros-deb-builder-action/issues/49) # - https://github.com/eclipse-zenoh/zenoh/pull/1150 (fix deadlock issue https://github.com/ros2/rmw_zenoh/issues/182) -ament_vendor(zenoh_c_vendor - VCS_URL https://github.com/eclipse-zenoh/zenoh-c - VCS_VERSION dev/1.0.0 - CMAKE_ARGS - "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" +# ament_vendor(zenoh_c_vendor +# VCS_URL https://github.com/eclipse-zenoh/zenoh-c +# VCS_VERSION dev/1.0.0 +# CMAKE_ARGS +# "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" +# ) + +include(ExternalProject) +set(ZENOHC_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}) +ExternalProject_Add(zenohc + PREFIX ${CMAKE_BINARY_DIR}/zenohc + GIT_REPOSITORY https://github.com/eclipse-zenoh/zenoh-c.git + GIT_TAG dev/1.0.0 + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${ZENOHC_INSTALL_DIR} -DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS} + BUILD_IN_SOURCE 1 +) +add_custom_target(zenoh_c_vendor ALL + DEPENDS zenohc ) +list(APPEND CMAKE_PREFIX_PATH ${ZENOHC_INSTALL_DIR}) -ament_package() +ament_package() \ No newline at end of file From ccd69a367d19f5b2cea8d773500c5c16ab2210e3 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 20 Aug 2024 08:57:09 +0300 Subject: [PATCH 22/41] merge fixes --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 4 ++-- rmw_zenoh_cpp/src/rmw_init.cpp | 14 -------------- 2 files changed, 2 insertions(+), 16 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index dbb8fd56..d39084ae 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -469,11 +469,11 @@ ZenohQuery::ZenohQuery(const z_loaned_query_t *query) { } ///============================================================================= -ZenohQuery::~ZenohQuery() { z_drop(z_move(query_)); } +ZenohQuery::~ZenohQuery() { z_drop(z_move(*query_)); } ///============================================================================= const z_loaned_query_t *ZenohQuery::get_query() const { - return z_query_loan(&query_); + return z_query_loan(query_); } //============================================================================== diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index b37e6036..da45fc50 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -93,10 +93,6 @@ graph_sub_data_handler(const z_loaned_sample_t * sample, void * data) //============================================================================== /// 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) { -<<<<<<< HEAD -======= - ->>>>>>> dfc95ade620952af587dc55464d0697efad15301 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, @@ -181,10 +177,6 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { return ret; } -<<<<<<< HEAD -======= - ->>>>>>> dfc95ade620952af587dc55464d0697efad15301 // TODO(yuyuan): SHM z_owned_string_t shm_enabled; zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled); @@ -229,10 +221,6 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { } } -<<<<<<< HEAD -======= - ->>>>>>> dfc95ade620952af587dc55464d0697efad15301 // TODO(yuyuan): SHM // Initialize the shm manager if shared_memory is enabled in the config. if (strncmp(z_string_data(z_loan(shm_enabled)), "true", z_string_len(z_loan(shm_enabled))) == 0) { @@ -382,8 +370,6 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { return RMW_RET_ERROR; } - z_drop(z_move(keyexpr)); - undeclare_z_sub.cancel(); close_session.cancel(); destruct_guard_condition_data.cancel(); From 8f36772055df22c1e28a986316356b689ade28a8 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 20 Aug 2024 11:01:19 +0300 Subject: [PATCH 23/41] SHM feature support for build system --- rmw_zenoh_cpp/CMakeLists.txt | 10 ++++++++++ zenoh_c_vendor/CMakeLists.txt | 6 ++++-- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index d2145006..95c2b2c2 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -7,6 +7,8 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() +set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY OFF CACHE BOOL "Compile Zenoh RMW with Shared Memory support") + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -77,6 +79,14 @@ target_compile_definitions(rmw_zenoh_cpp RMW_VERSION_PATCH=${rmw_VERSION_PATCH} ) +if(${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}) + target_compile_definitions(rmw_zenoh_cpp + PRIVATE + RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + ) +endif() + + ament_export_targets(export_rmw_zenoh_cpp) register_rmw_implementation( diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 943ebb1d..2a247363 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -15,7 +15,9 @@ find_package(ament_cmake_vendor_package REQUIRED) # Note: We separate the two args needed for cargo with "$" and not ";" as the # latter is a list separater in cmake and hence the string will be split into two # when expanded. -set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memory zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") +set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") + +set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY OFF CACHE BOOL "Compile Zenoh RMW with Shared Memory support") # Set VCS_VERSION to include latest changes from zenoh-c to benefit from : # - https://github.com/eclipse-zenoh/zenoh-c/pull/340 (fix build issue) @@ -27,7 +29,7 @@ ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c VCS_VERSION dev/1.0.0 CMAKE_ARGS - "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" + "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}; -DZENOHC_BUILD_WITH_SHARED_MEMORY=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}; -DZENOHC_BUILD_WITH_UNSTABLE_API=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}" ) ament_package() From 9fac5ee3c02d7c303468fbc314ac73553d08cd10 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 20 Aug 2024 11:32:33 +0300 Subject: [PATCH 24/41] Hide all shm stuff behind #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY --- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 3 +- rmw_zenoh_cpp/src/rmw_init.cpp | 13 +++++++-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 31 +++++++++++++++------ 3 files changed, 36 insertions(+), 11 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 0b314d35..78308a86 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -52,8 +52,9 @@ class rmw_context_impl_s final // An owned session. z_owned_session_t session; - // TODO(yuyuan): SHM +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY std::optional shm_provider; +#endif z_owned_subscriber_t graph_subscriber; diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index da45fc50..5a4182ec 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -41,9 +41,11 @@ extern "C" { +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // Megabytes of SHM to reserve. // TODO(clalancette): Make this configurable, or get it from the configuration #define SHM_BUFFER_SIZE_MB 10 +#endif namespace { @@ -177,13 +179,14 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { return ret; } - // TODO(yuyuan): SHM +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY z_owned_string_t shm_enabled; zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled); auto free_shm_= rcpputils::make_scope_exit( [&shm_enabled]() { z_drop(z_move(shm_enabled)); }); +#endif // Initialize the zenoh session. if(z_open(&context->impl->session, z_move(config))) { @@ -221,7 +224,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { } } - // TODO(yuyuan): SHM +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // Initialize the shm manager if shared_memory is enabled in the config. if (strncmp(z_string_data(z_loan(shm_enabled)), "true", z_string_len(z_loan(shm_enabled))) == 0) { printf(">>> SHM is enabled\n"); @@ -245,6 +248,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { z_drop(z_move(context->impl->shm_provider.value())); } }); +#endif // Initialize the guard condition. context->impl->graph_guard_condition = @@ -380,7 +384,9 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { free_options.cancel(); impl_destructor.cancel(); free_impl.cancel(); +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY free_shm_provider.cancel(); +#endif restore_context.cancel(); return RMW_RET_OK; @@ -397,9 +403,12 @@ rmw_ret_t rmw_shutdown(rmw_context_t *context) { return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + // Drop SHM provider if (context->impl->shm_provider.has_value()) { z_drop(z_move(context->impl->shm_provider.value())); } +#endif // 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 efded20a..8588e23b 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -161,8 +161,10 @@ find_service_type_support(const rosidl_service_type_support_t *type_supports) { extern "C" { +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // TODO(yuyuan): SHM, make this configurable #define SHM_BUF_OK_SIZE 2621440 +#endif //============================================================================== /// Get the name of the rmw implementation being used @@ -885,22 +887,31 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // To store serialized message byte array. char *msg_bytes = nullptr; - // TODO(yuyuan): SHM +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY std::optional shmbuf = std::nullopt; auto always_free_shmbuf = rcpputils::make_scope_exit([&shmbuf]() { if (shmbuf.has_value()) { - // TODO(yuyuan): do we need to drop z_owned_shm_mut_t? z_drop(z_move(shmbuf.value())); } }); +#endif + auto free_msg_bytes = - rcpputils::make_scope_exit([&msg_bytes, allocator, &shmbuf]() { - if (msg_bytes && !shmbuf.has_value()) { + rcpputils::make_scope_exit([&msg_bytes, allocator +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , &shmbuf +#endif + ]() { + if (msg_bytes +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + && !shmbuf.has_value() +#endif + ) { allocator->deallocate(msg_bytes, allocator->state); } }); - // TODO(yuyuan): SHM +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // Get memory from SHM buffer if available. if (publisher_data->context->impl->shm_provider.has_value()) { // printf(">>> rmw_publish(), SHM enabled\n"); @@ -923,7 +934,9 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, return RMW_RET_ERROR; } - } else { + } else +#endif + { // printf(">>> rmw_publish(), SHM disabled\n"); // Get memory from the allocator. @@ -966,11 +979,13 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, z_owned_bytes_t payload; - // TODO(yuyuan): SHM +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY if (shmbuf.has_value()) { z_bytes_serialize_from_shm_mut(&payload, z_move(shmbuf.value())); z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options); - } else { + } else +#endif + { z_bytes_serialize_from_buf( &payload, reinterpret_cast(msg_bytes), data_length); if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options)) { From d2cccf2317a3c78d4906d9e7553becca5bebec40 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 20 Aug 2024 11:37:14 +0300 Subject: [PATCH 25/41] Update .gitignore --- .gitignore | 1 + rmw_zenoh_cpp/.vscode/settings.json | 74 ----------------------------- 2 files changed, 1 insertion(+), 74 deletions(-) delete mode 100644 rmw_zenoh_cpp/.vscode/settings.json diff --git a/.gitignore b/.gitignore index 01716761..154b5f98 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ build install .cache log +.vscode diff --git a/rmw_zenoh_cpp/.vscode/settings.json b/rmw_zenoh_cpp/.vscode/settings.json deleted file mode 100644 index ed4a30f5..00000000 --- a/rmw_zenoh_cpp/.vscode/settings.json +++ /dev/null @@ -1,74 +0,0 @@ -{ - "files.associations": { - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "array": "cpp", - "atomic": "cpp", - "strstream": "cpp", - "bit": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "cinttypes": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "forward_list": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "fstream": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "semaphore": "cpp", - "shared_mutex": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cfenv": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "variant": "cpp" - } -} \ No newline at end of file From 785eab045536030f60fd89f59b248c40fcb282b4 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 20 Aug 2024 11:40:46 +0300 Subject: [PATCH 26/41] Fix owned query handling --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 6 +++--- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index d39084ae..0d288964 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -465,15 +465,15 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) { ///============================================================================= ZenohQuery::ZenohQuery(const z_loaned_query_t *query) { - z_query_clone(query_, query); + z_query_clone(&query_, query); } ///============================================================================= -ZenohQuery::~ZenohQuery() { z_drop(z_move(*query_)); } +ZenohQuery::~ZenohQuery() { z_drop(z_move(query_)); } ///============================================================================= const z_loaned_query_t *ZenohQuery::get_query() const { - return z_query_loan(query_); + return z_query_loan(&query_); } //============================================================================== diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 78308a86..574e0d76 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -217,7 +217,7 @@ class ZenohQuery final const z_loaned_query_t * get_query() const; private: - z_owned_query_t * query_{nullptr}; + z_owned_query_t query_; }; ///============================================================================= From aace581c78e900ed1cc4036ad483d03a6730aef4 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Sun, 18 Aug 2024 22:18:54 +0800 Subject: [PATCH 27/41] fix: segmentation fault due to the unallocated query memory --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 6 +++--- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 2 +- rmw_zenoh_cpp/src/rmw_init.cpp | 2 -- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 21 +++++++-------------- 4 files changed, 11 insertions(+), 20 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index d39084ae..0d288964 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -465,15 +465,15 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) { ///============================================================================= ZenohQuery::ZenohQuery(const z_loaned_query_t *query) { - z_query_clone(query_, query); + z_query_clone(&query_, query); } ///============================================================================= -ZenohQuery::~ZenohQuery() { z_drop(z_move(*query_)); } +ZenohQuery::~ZenohQuery() { z_drop(z_move(query_)); } ///============================================================================= const z_loaned_query_t *ZenohQuery::get_query() const { - return z_query_loan(query_); + return z_query_loan(&query_); } //============================================================================== diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 0b314d35..b6744971 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -216,7 +216,7 @@ class ZenohQuery final const z_loaned_query_t * get_query() const; private: - z_owned_query_t * query_{nullptr}; + z_owned_query_t query_; }; ///============================================================================= diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 66f67b67..2a8f8772 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -386,8 +386,6 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { free_shm_provider.cancel(); restore_context.cancel(); - - return RMW_RET_OK; } diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 26689c5f..662ff494 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1416,17 +1416,14 @@ rmw_subscription_t *rmw_create_subscription( sub_options.reliability = Z_RELIABILITY_RELIABLE; } - ; - - ze_owned_querying_subscriber_t *sub = - &std::get(sub_data->sub); - ze_declare_querying_subscriber(sub, z_loan(context_impl->session), + ze_owned_querying_subscriber_t sub; + if (ze_declare_querying_subscriber(&sub, z_loan(context_impl->session), z_loan(keyexpr), z_move(callback), - &sub_options); - if (!z_check(*sub)) { + &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } + sub_data->sub = sub; } else { // Create a regular subscriber for all other durability settings. z_subscriber_options_t sub_options; @@ -1435,13 +1432,12 @@ rmw_subscription_t *rmw_create_subscription( sub_options.reliability = Z_RELIABILITY_RELIABLE; } - z_owned_subscriber_t *sub = &std::get(sub_data->sub); - z_declare_subscriber(sub, z_loan(context_impl->session), z_loan(keyexpr), - z_move(callback), &sub_options); - if (!z_check(*sub)) { + z_owned_subscriber_t sub; + if(z_declare_subscriber(&sub, z_loan(context_impl->session), z_loan(keyexpr), z_move(callback), &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } + sub_data->sub = sub; } auto undeclare_z_sub = rcpputils::make_scope_exit([sub_data]() { @@ -2374,9 +2370,6 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, z_owned_closure_reply_t callback; z_closure(&callback, rmw_zenoh_cpp::client_data_handler, NULL, client_data); - - // TODO(yuyuan): z_owned_closure_reply_t zn_closure_reply is replaced with a - // moved callback z_get(z_loan(context_impl->session), z_loan(client_data->keyexpr), "", z_move(callback), &opts); From 57fb755e21f0b9c0997e7fb2a560aee9dde1a378 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 21 Aug 2024 11:27:03 +0300 Subject: [PATCH 28/41] configurable shm size --- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 35 +++++++++++++++++++++++ rmw_zenoh_cpp/src/detail/zenoh_config.hpp | 11 +++++++ rmw_zenoh_cpp/src/rmw_init.cpp | 7 +---- 3 files changed, 47 insertions(+), 6 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 5e205196..a1445122 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -42,6 +42,10 @@ static const std::unordered_map zenoh_router_check_attempts() // If unset, check indefinitely. return default_value; } + + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +///============================================================================= +size_t zenoh_shm_alloc_size() { + const char * envar_value; + + if (NULL != rcutils_get_env(zenoh_shm_alloc_size_envar, &envar_value)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s cannot be read. Report this bug.", + zenoh_shm_alloc_size_envar); + return zenoh_shm_alloc_size_default; + } + + // If the environment variable contains a value, handle it accordingly. + if (envar_value[0] != '\0') { + const auto read_value = std::strtoull(envar_value, nullptr, 10); + if (read_value > 0) { + if (read_value > std::numeric_limits::max()) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s: value is too large!", + zenoh_shm_alloc_size_envar); + } else { + return read_value; + } + } + } + + return zenoh_shm_alloc_size_default; +} +#endif } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp index 357c7e89..141b262b 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -57,6 +57,17 @@ rmw_ret_t get_z_config(const ConfigurableEntity & entity, z_owned_config_t * con /// @return The number of times to try connecting to a zenoh router and /// std::nullopt if establishing a connection to a router is not required. std::optional zenoh_router_check_attempts(); + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +///============================================================================= +/// Get the amount of shared memory to be pre-allocated for Zenoh SHM operation +/// based on the environment variable ZENOH_SHM_ALLOC_SIZE. +/// @details The behavior is as follows: +/// - If not set or <= 0, the default value of 1MB is returned. +/// - Else value of environemnt variable is returned. +/// @return The amount of shared memory to be pre-allocated for Zenoh SHM operation +size_t zenoh_shm_alloc_size(); +#endif } // namespace rmw_zenoh_cpp #endif // DETAIL__ZENOH_CONFIG_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 5a4182ec..5e81defe 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -41,11 +41,6 @@ extern "C" { -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY -// Megabytes of SHM to reserve. -// TODO(clalancette): Make this configurable, or get it from the configuration -#define SHM_BUFFER_SIZE_MB 10 -#endif namespace { @@ -232,7 +227,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { // TODO(yuyuan): determine the default alignment z_alloc_alignment_t alignment = {5}; z_owned_memory_layout_t layout; - z_memory_layout_new(&layout, SHM_BUFFER_SIZE_MB * 1024 * 1024, alignment); + z_memory_layout_new(&layout, rmw_zenoh_cpp::zenoh_shm_alloc_size(), alignment); z_owned_shm_provider_t provider; if (z_posix_shm_provider_new(&provider, z_loan(layout))) { From 5ef8a705cbac4beb136d44e076de45a4420ffa05 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 22 Aug 2024 10:34:24 +0300 Subject: [PATCH 29/41] fix Zenoh Config read\check --- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 5e205196..206e2a92 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -57,18 +57,13 @@ rmw_ret_t _get_z_config( "rmw_zenoh_cpp", "Envar %s cannot be read.", envar_name); return RMW_RET_ERROR; } - // If the environment variable contains a path to a file, try to read the configuration from it. - if (envar_uri[0] != '\0') { - // If the environment variable is set, try to read the configuration from the file. - zc_config_from_file(config, envar_uri); - configured_uri = envar_uri; - } else { - // If the environment variable is not set use internal configuration - zc_config_from_file(config, default_uri); - configured_uri = default_uri; - } - // Verify that the configuration is valid. - if (!z_config_check(config)) { + + // If the environment variable is set, try to read the configuration from the file, + // if the environment variable is not set use internal configuration + configured_uri = envar_uri[0] != '\0' ? envar_uri : default_uri; + + // Try to read the configuration + if (zc_config_from_file(config, configured_uri) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Invalid configuration file %s", configured_uri); From be31a20eda3ee9325dff022df2d1480a243d274c Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 23 Aug 2024 17:32:40 +0300 Subject: [PATCH 30/41] adopt to recent zenoh-c API changes --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 16 +- rmw_zenoh_cpp/src/rmw_init.cpp | 15 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 161 +++++++------------- 3 files changed, 66 insertions(+), 126 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 0d288964..780d644c 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -528,7 +528,12 @@ void client_data_handler(const z_loaned_reply_t *reply, void *data) { return; } - if (!z_reply_is_ok(reply)) { + if (z_reply_is_ok(reply)) + { + z_owned_reply_t owned_reply; + z_reply_clone(&owned_reply, reply); + client_data->add_new_reply(std::make_unique(&owned_reply)); + } else { z_view_string_t keystr; z_keyexpr_as_view_string(z_loan(client_data->keyexpr), &keystr); const z_loaned_reply_err_t *err = z_reply_err(reply); @@ -545,15 +550,6 @@ void client_data_handler(const z_loaned_reply_t *reply, void *data) { z_drop(z_move(err_str)); return; } - z_owned_reply_t owned_reply; - z_reply_clone(&owned_reply, reply); - - if (!z_reply_check(&owned_reply)) { - RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "z_reply_check returned False"); - return; - } - - client_data->add_new_reply(std::make_unique(&owned_reply)); } void client_data_drop(void *data) { diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 2a8f8772..7526b7a7 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -355,25 +355,14 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { z_view_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); - zc_liveliness_declare_subscriber( + if(zc_liveliness_declare_subscriber( &context->impl->graph_subscriber, z_loan(context->impl->session), z_loan(keyexpr), - z_move(callback), &sub_options); - - const z_loaned_keyexpr_t *sub_ke = z_subscriber_keyexpr(z_loan(context->impl->graph_subscriber)); - z_view_string_t sub_keyexpr; - z_keyexpr_as_view_string(sub_ke, &sub_keyexpr); - - - 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)) { + z_move(callback), &sub_options) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return RMW_RET_ERROR; } - undeclare_z_sub.cancel(); close_session.cancel(); destruct_guard_condition_data.cancel(); impl_destructor.cancel(); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 662ff494..7f18f0b7 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -85,10 +85,11 @@ std::string strip_slashes(const char *const str) { // 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. -z_owned_keyexpr_t ros_topic_name_to_zenoh_key(const std::size_t domain_id, - const char *const topic_name, - const char *const topic_type, - const char *const type_hash) { +bool ros_topic_name_to_zenoh_key(z_owned_keyexpr_t* out_keyexpr, + const std::size_t domain_id, + const char *const topic_name, + const char *const topic_type, + const char *const type_hash) { std::string keyexpr_str = std::to_string(domain_id); keyexpr_str += "/"; keyexpr_str += strip_slashes(topic_name); @@ -98,9 +99,7 @@ z_owned_keyexpr_t ros_topic_name_to_zenoh_key(const std::size_t domain_id, keyexpr_str += type_hash; // TODO(yuyuan): use z_view_keyexpr_t? - z_owned_keyexpr_t keyexpr; - z_keyexpr_from_str(&keyexpr, keyexpr_str.c_str()); - return keyexpr; + return z_keyexpr_from_str(out_keyexpr, keyexpr_str.c_str()) == Z_OK; } //============================================================================== @@ -311,11 +310,6 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, auto free_token = rcpputils::make_scope_exit( [node_data]() { z_drop(z_move(node_data->token)); }); - if (!z_check(node_data->token)) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Unable to create liveliness token for the node."); - return nullptr; - } node->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; node->context = context; @@ -476,10 +470,6 @@ rmw_publisher_t *rmw_create_publisher( return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG(context_impl->enclave, "expected initialized enclave", return nullptr); - if (!z_check(context_impl->session)) { - RMW_SET_ERROR_MSG("zenoh session is invalid"); - return nullptr; - } rcutils_allocator_t *allocator = &node->context->options.allocator; @@ -580,17 +570,17 @@ rmw_publisher_t *rmw_create_publisher( allocator->deallocate(type_hash_c_str, allocator->state); }); - z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key( + z_owned_keyexpr_t keyexpr; + if (!ros_topic_name_to_zenoh_key( + &keyexpr, node->context->actual_domain_id, topic_name, - publisher_data->type_support->get_name(), type_hash_c_str); - z_view_string_t str; - z_keyexpr_as_view_string(z_loan(keyexpr), &str); + publisher_data->type_support->get_name(), type_hash_c_str)) { + RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); + return nullptr; + } + 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; - } // Create a Publication Cache if durability is transient_local. if (publisher_data->adapted_qos_profile.durability == @@ -794,15 +784,14 @@ rmw_return_loaned_message_from_publisher(const rmw_publisher_t *publisher, } namespace { -z_owned_bytes_t -create_map_and_set_sequence_num(int64_t sequence_number, +bool +create_map_and_set_sequence_num(z_owned_bytes_t* out_bytes, + int64_t sequence_number, uint8_t gid[RMW_GID_STORAGE_SIZE]) { // z_owned_slice_map_t map; // z_slice_map_new(&map); - z_owned_bytes_t bytes; - // auto free_attachment_map = // rcpputils::make_scope_exit([&map]() { z_drop(z_move(map)); }); @@ -845,14 +834,13 @@ create_map_and_set_sequence_num(int64_t sequence_number, rmw_zenoh_cpp::attachement_data_t data(sequence_number, source_timestamp, gid); - if (data.serialize_to_zbytes(&bytes)) { + if (data.serialize_to_zbytes(out_bytes)) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Failed to serialize the attachment"); - z_bytes_null(&bytes); - return bytes; + return false; } - return bytes; + return true; } } // namespace @@ -916,7 +904,7 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, &alloc, z_loan(provider), SHM_BUF_OK_SIZE, alignment); - if (z_check(alloc.buf)) { + if (alloc.status == ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { shmbuf = std::make_optional(alloc.buf); msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); @@ -951,9 +939,8 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, int64_t sequence_number = publisher_data->get_next_sequence_number(); - z_owned_bytes_t attachment = - create_map_and_set_sequence_num(sequence_number, publisher_data->pub_gid); - if (!z_check(attachment)) { + z_owned_bytes_t attachment; + if (!create_map_and_set_sequence_num(&attachment, sequence_number, publisher_data->pub_gid)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } @@ -1073,10 +1060,8 @@ rmw_ret_t rmw_publish_serialized_message( uint64_t sequence_number = publisher_data->get_next_sequence_number(); - z_owned_bytes_t attachment = - create_map_and_set_sequence_num(sequence_number, publisher_data->pub_gid); - - if (!z_check(attachment)) { + z_owned_bytes_t attachment; + if (!create_map_and_set_sequence_num(&attachment, sequence_number, publisher_data->pub_gid)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } @@ -1126,10 +1111,6 @@ rmw_ret_t rmw_publisher_assert_liveliness(const rmw_publisher_t *publisher) { static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); - if (!zc_liveliness_token_check(&pub_data->token)) { - return RMW_RET_ERROR; - } - return RMW_RET_OK; } @@ -1275,10 +1256,6 @@ rmw_subscription_t *rmw_create_subscription( return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG(context_impl->enclave, "expected initialized enclave", return nullptr); - if (!z_check(context_impl->session)) { - RMW_SET_ERROR_MSG("zenoh session is invalid"); - return nullptr; - } rcutils_allocator_t *allocator = &node->context->options.allocator; @@ -1382,15 +1359,16 @@ rmw_subscription_t *rmw_create_subscription( // with Zenoh; after this, callbacks may come in at any time. z_owned_closure_sample_t callback; z_closure(&callback, rmw_zenoh_cpp::sub_data_handler, nullptr, sub_data); - z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key( + z_owned_keyexpr_t keyexpr; + if (!ros_topic_name_to_zenoh_key( + &keyexpr, node->context->actual_domain_id, topic_name, - sub_data->type_support->get_name(), type_hash_c_str); - auto always_free_ros_keyexpr = rcpputils::make_scope_exit( - [&keyexpr]() { z_keyexpr_drop(z_move(keyexpr)); }); - if (!z_keyexpr_check(&keyexpr)) { + sub_data->type_support->get_name(), type_hash_c_str)) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } + auto always_free_ros_keyexpr = rcpputils::make_scope_exit( + [&keyexpr]() { z_keyexpr_drop(z_move(keyexpr)); }); // // TODO(yuyuan): owned_key_str seems to be useless here? // // Instantiate the subscription with suitable options depending on the @@ -1484,14 +1462,8 @@ rmw_subscription_t *rmw_create_subscription( RMW_SET_ERROR_MSG("unable to create zenoh keyexpr for liveness."); return nullptr; } - zc_liveliness_declare_token(&sub_data->token, z_loan(context_impl->session), - z_loan(token_keyexpr), NULL); - auto free_token = rcpputils::make_scope_exit([sub_data]() { - if (sub_data != nullptr) { - z_drop(z_move(sub_data->token)); - } - }); - if (!z_check(sub_data->token)) { + if(zc_liveliness_declare_token(&sub_data->token, z_loan(context_impl->session), + z_loan(token_keyexpr), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the subscription."); @@ -1500,7 +1472,6 @@ rmw_subscription_t *rmw_create_subscription( rmw_subscription->data = sub_data; - free_token.cancel(); undeclare_z_sub.cancel(); free_topic_name.cancel(); destruct_msg_type_support.cancel(); @@ -1981,10 +1952,7 @@ rmw_client_t *rmw_create_client( return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG(context_impl->enclave, "expected initialized enclave", return nullptr); - if (!z_check(context_impl->session)) { - RMW_SET_ERROR_MSG("zenoh session is invalid"); - return nullptr; - } + RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); const rmw_zenoh_cpp::rmw_node_data_t *node_data = static_cast(node->data); @@ -2155,16 +2123,16 @@ rmw_client_t *rmw_create_client( allocator->deallocate(type_hash_c_str, allocator->state); }); - client_data->keyexpr = ros_topic_name_to_zenoh_key( + if(!ros_topic_name_to_zenoh_key( + &client_data->keyexpr, node->context->actual_domain_id, rmw_client->service_name, - service_type.c_str(), type_hash_c_str); - auto free_ros_keyexpr = rcpputils::make_scope_exit( - [client_data]() { z_keyexpr_drop(z_move(client_data->keyexpr)); }); - if (!z_keyexpr_check(&client_data->keyexpr)) { + service_type.c_str(), type_hash_c_str)) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } - + auto free_ros_keyexpr = rcpputils::make_scope_exit( + [client_data]() { z_keyexpr_drop(z_move(client_data->keyexpr)); }); + client_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), std::to_string(node_data->id), @@ -2184,31 +2152,24 @@ rmw_client_t *rmw_create_client( } z_owned_keyexpr_t keyexpr; z_keyexpr_from_str(&keyexpr, client_data->entity->keyexpr().c_str()); - zc_liveliness_declare_token(&client_data->token, + if (zc_liveliness_declare_token(&client_data->token, z_loan(node->context->impl->session), - z_loan(keyexpr), NULL); + z_loan(keyexpr), NULL) != Z_OK) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Unable to create liveliness token for the client."); + return nullptr; + } // WARN(yuyuan): z_view_keyexpr_t would fail // z_view_keyexpr_t keyexpr; // z_view_keyexpr_from_str(&keyexpr, client_data->entity->keyexpr().c_str()); // zc_liveliness_declare_token(&client_data->token, // z_loan(node->context->impl->session), // z_loan(keyexpr), NULL); - auto free_token = rcpputils::make_scope_exit([client_data]() { - if (client_data != nullptr) { - z_drop(z_move(client_data->token)); - } - }); - if (!z_check(client_data->token)) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Unable to create liveliness token for the client."); - return nullptr; - } - z_drop(z_move(keyexpr)); rmw_client->data = client_data; - free_token.cancel(); free_rmw_client.cancel(); free_client_data.cancel(); destruct_request_type_support.cancel(); @@ -2334,9 +2295,8 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, z_get_options_t opts; z_get_options_default(&opts); - z_owned_bytes_t attachment = - create_map_and_set_sequence_num(*sequence_id, client_data->client_gid); - if (!z_check(attachment)) { + z_owned_bytes_t attachment; + if (!create_map_and_set_sequence_num(&attachment, *sequence_id, client_data->client_gid)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } @@ -2550,10 +2510,6 @@ rmw_service_t *rmw_create_service( return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG(context_impl->enclave, "expected initialized enclave", return nullptr); - if (!z_check(context_impl->session)) { - RMW_SET_ERROR_MSG("zenoh session is invalid"); - return nullptr; - } // SERVICE DATA ============================================================== rcutils_allocator_t *allocator = &node->context->options.allocator; @@ -2698,20 +2654,19 @@ rmw_service_t *rmw_create_service( allocator->deallocate(type_hash_c_str, allocator->state); }); - service_data->keyexpr = ros_topic_name_to_zenoh_key( + if(!ros_topic_name_to_zenoh_key( + &service_data->keyexpr, node->context->actual_domain_id, rmw_service->service_name, - service_type.c_str(), type_hash_c_str); + service_type.c_str(), type_hash_c_str)) { + RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); + return nullptr; + } auto free_ros_keyexpr = rcpputils::make_scope_exit([service_data]() { if (service_data) { z_drop(z_move(service_data->keyexpr)); } }); - if (!z_keyexpr_check(&service_data->keyexpr)) { - RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); - return nullptr; - } - z_owned_closure_query_t callback; z_closure(&callback, rmw_zenoh_cpp::service_data_handler, nullptr, service_data); @@ -3009,9 +2964,9 @@ rmw_ret_t rmw_send_response(const rmw_service_t *service, z_query_reply_options_t options; z_query_reply_options_default(&options); - z_owned_bytes_t attachment = create_map_and_set_sequence_num( - request_header->sequence_number, request_header->writer_guid); - if (!z_check(attachment)) { + z_owned_bytes_t attachment; + if (!create_map_and_set_sequence_num(&attachment, + request_header->sequence_number, request_header->writer_guid)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } From 2097c3068f46f344f67e4e99d18bf336602c3b29 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 23 Aug 2024 18:16:20 +0300 Subject: [PATCH 31/41] fix SHM feature passing through build system --- 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 2a247363..12da2b2b 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -29,7 +29,7 @@ ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c VCS_VERSION dev/1.0.0 CMAKE_ARGS - "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}; -DZENOHC_BUILD_WITH_SHARED_MEMORY=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}; -DZENOHC_BUILD_WITH_UNSTABLE_API=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}" + -DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS} -DZENOHC_BUILD_WITH_SHARED_MEMORY=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY} -DZENOHC_BUILD_WITH_UNSTABLE_API=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY} ) ament_package() From 94a014a899bda3b09a7bb81f0222399209648c8d Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 26 Aug 2024 22:01:07 +0300 Subject: [PATCH 32/41] - refine SHM code, fix some bugs - refine build system --- rmw_zenoh_cpp/src/rmw_init.cpp | 17 ++++++++--- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 51 ++++++++++++++++++--------------- zenoh_c_vendor/CMakeLists.txt | 4 ++- 3 files changed, 44 insertions(+), 28 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 688d5b8f..894d6d0b 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -224,16 +224,25 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { if (strncmp(z_string_data(z_loan(shm_enabled)), "true", z_string_len(z_loan(shm_enabled))) == 0) { printf(">>> SHM is enabled\n"); - // TODO(yuyuan): determine the default alignment - z_alloc_alignment_t alignment = {5}; + // Create Layout for provider's memory + // Provider's alignment will be 1 byte as we are going to make only 1-byte aligned allocations + z_alloc_alignment_t alignment = {0}; z_owned_memory_layout_t layout; - z_memory_layout_new(&layout, rmw_zenoh_cpp::zenoh_shm_alloc_size(), alignment); + if(z_memory_layout_new(&layout, rmw_zenoh_cpp::zenoh_shm_alloc_size(), alignment) != Z_OK) { + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a Layout for SHM provider."); + return RMW_RET_ERROR; + } + // Create SHM provider z_owned_shm_provider_t provider; - if (z_posix_shm_provider_new(&provider, z_loan(layout))) { + const auto provider_creation_result = z_posix_shm_provider_new(&provider, z_loan(layout)); + z_drop(layout); + if (provider_creation_result != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a SHM provider."); return RMW_RET_ERROR; } + + // Upon successful provider creation, store it in the context context->impl->shm_provider = std::make_optional(provider); } diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 0bc7f4c3..a076cad2 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -160,11 +160,6 @@ find_service_type_support(const rosidl_service_type_support_t *type_supports) { extern "C" { -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY -// TODO(yuyuan): SHM, make this configurable -#define SHM_BUF_OK_SIZE 2621440 -#endif - //============================================================================== /// Get the name of the rmw implementation being used const char *rmw_get_implementation_identifier(void) { @@ -904,13 +899,15 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, if (publisher_data->context->impl->shm_provider.has_value()) { // printf(">>> rmw_publish(), SHM enabled\n"); - auto provider = publisher_data->context->impl->shm_provider.value(); + const auto& provider = publisher_data->context->impl->shm_provider.value(); + + // Allocate SHM bufer + // We use 1-byte alignment z_buf_layout_alloc_result_t alloc; - // TODO(yuyuan): SHM, configure this - z_alloc_alignment_t alignment = {5}; + z_alloc_alignment_t alignment = {0}; z_shm_provider_alloc_gc_defrag_blocking( &alloc, z_loan(provider), - SHM_BUF_OK_SIZE, alignment); + max_data_length, alignment); if (alloc.status == ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { shmbuf = std::make_optional(alloc.buf); @@ -957,30 +954,38 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, auto free_attachment = rcpputils::make_scope_exit( [&attachment]() { z_drop(z_move(attachment)); }); - // 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); - options.attachment = z_move(attachment); - z_owned_bytes_t payload; - #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY if (shmbuf.has_value()) { - z_bytes_serialize_from_shm_mut(&payload, z_move(shmbuf.value())); - z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options); + if (z_bytes_serialize_from_shm_mut(&payload, z_move(shmbuf.value())) != Z_OK) { + RMW_SET_ERROR_MSG("unable to serialize SHM buffer into Zenoh Payload"); + return RMW_RET_ERROR; + } } else #endif { - z_bytes_serialize_from_buf( - &payload, reinterpret_cast(msg_bytes), data_length); - if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options)) { - RMW_SET_ERROR_MSG("unable to publish message"); + if (z_bytes_serialize_from_buf( + &payload, reinterpret_cast(msg_bytes), data_length) != Z_OK) { + RMW_SET_ERROR_MSG("unable to serialize raw buffer into Zenoh Payload"); return RMW_RET_ERROR; } } + // we cancel free attachment because attachment will be moved into z_publisher_put_options_t + free_attachment.cancel(); + + // 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); + options.attachment = z_move(attachment); + + if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options) != Z_OK) { + RMW_SET_ERROR_MSG("unable to publish message"); + return RMW_RET_ERROR; + } + return RMW_RET_OK; } diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 12da2b2b..db575f69 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -29,7 +29,9 @@ ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c VCS_VERSION dev/1.0.0 CMAKE_ARGS - -DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS} -DZENOHC_BUILD_WITH_SHARED_MEMORY=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY} -DZENOHC_BUILD_WITH_UNSTABLE_API=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY} + -DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS} + -DZENOHC_BUILD_WITH_SHARED_MEMORY=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY} + -DZENOHC_BUILD_WITH_UNSTABLE_API=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY} ) ament_package() From cadd258ce51ca6cb7c24c2df72a2a0887f2f74f9 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 27 Aug 2024 00:09:38 +0300 Subject: [PATCH 33/41] Add configurable message size threshold for handling ROS messages through SHM --- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 6 ++++- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 29 +++++++++++++++++++++ rmw_zenoh_cpp/src/detail/zenoh_config.hpp | 9 +++++++ rmw_zenoh_cpp/src/rmw_init.cpp | 20 ++++++++------ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 9 ++++--- 5 files changed, 60 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 574e0d76..54b0bd0e 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -53,7 +53,11 @@ class rmw_context_impl_s final z_owned_session_t session; #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - std::optional shm_provider; + struct rmw_shm_s { + z_owned_shm_provider_t shm_provider; + size_t msgsize_threshold; + }; + std::optional shm; #endif z_owned_subscriber_t graph_subscriber; diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index c6863598..4f4bf4a5 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -45,6 +45,8 @@ static const char * router_check_attempts_envar = "ZENOH_ROUTER_CHECK_ATTEMPTS"; #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY static const char * zenoh_shm_alloc_size_envar = "ZENOH_SHM_ALLOC_SIZE"; static const size_t zenoh_shm_alloc_size_default = 1024*1024; +static const char * zenoh_shm_message_size_threshold_envar = "ZENOH_SHM_MESSAGE_SIZE_THRESHOLD"; +static const size_t zenoh_shm_message_size_threshold_default = 2*1024; #endif rmw_ret_t _get_z_config( @@ -157,5 +159,32 @@ size_t zenoh_shm_alloc_size() { return zenoh_shm_alloc_size_default; } +///============================================================================= +size_t zenoh_shm_message_size_threshold() { + const char * envar_value; + + if (NULL != rcutils_get_env(zenoh_shm_message_size_threshold_envar, &envar_value)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s cannot be read. Report this bug.", + zenoh_shm_message_size_threshold_envar); + return zenoh_shm_message_size_threshold_default; + } + + // If the environment variable contains a value, handle it accordingly. + if (envar_value[0] != '\0') { + const auto read_value = std::strtoull(envar_value, nullptr, 10); + if (read_value > 0) { + if (read_value > std::numeric_limits::max()) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s: value is too large!", + zenoh_shm_message_size_threshold_envar); + } else { + return read_value; + } + } + } + + return zenoh_shm_message_size_threshold_default; +} #endif } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp index 141b262b..ae4cf74e 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -67,6 +67,15 @@ std::optional zenoh_router_check_attempts(); /// - Else value of environemnt variable is returned. /// @return The amount of shared memory to be pre-allocated for Zenoh SHM operation size_t zenoh_shm_alloc_size(); +///============================================================================= +/// Message size threshold for implicit SHM optimization based on the environment +/// variable ZENOH_SHM_ALLOC_SIZE. +/// Messages smaller than this threshold will not be forwarded through Zenoh SHM +/// @details The behavior is as follows: +/// - If not set or <= 0, the default value of 2KB is returned. +/// - Else value of environemnt variable is returned. +/// @return The amount of shared memory to be pre-allocated for Zenoh SHM operation +size_t zenoh_shm_message_size_threshold(); #endif } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 894d6d0b..086cf9eb 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -224,6 +224,11 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { if (strncmp(z_string_data(z_loan(shm_enabled)), "true", z_string_len(z_loan(shm_enabled))) == 0) { printf(">>> SHM is enabled\n"); + rmw_context_impl_s::rmw_shm_s shm; + + // Read msg size treshold from config + shm.msgsize_threshold = rmw_zenoh_cpp::zenoh_shm_message_size_threshold(); + // Create Layout for provider's memory // Provider's alignment will be 1 byte as we are going to make only 1-byte aligned allocations z_alloc_alignment_t alignment = {0}; @@ -234,22 +239,21 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { } // Create SHM provider - z_owned_shm_provider_t provider; - const auto provider_creation_result = z_posix_shm_provider_new(&provider, z_loan(layout)); - z_drop(layout); + const auto provider_creation_result = z_posix_shm_provider_new(&shm.shm_provider, z_loan(layout)); + z_drop(z_move(layout)); if (provider_creation_result != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a SHM provider."); return RMW_RET_ERROR; } // Upon successful provider creation, store it in the context - context->impl->shm_provider = std::make_optional(provider); + context->impl->shm = std::make_optional(shm); } auto free_shm_provider = rcpputils::make_scope_exit( [context]() { - if (context->impl->shm_provider.has_value()) { - z_drop(z_move(context->impl->shm_provider.value())); + if (context->impl->shm.has_value()) { + z_drop(z_move(context->impl->shm.value().shm_provider)); } }); #endif @@ -398,8 +402,8 @@ rmw_ret_t rmw_shutdown(rmw_context_t *context) { z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // Drop SHM provider - if (context->impl->shm_provider.has_value()) { - z_drop(z_move(context->impl->shm_provider.value())); + if (context->impl->shm.has_value()) { + z_drop(z_move(context->impl->shm.value().shm_provider)); } #endif // Close the zenoh session diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index a076cad2..18851c41 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -896,10 +896,11 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // Get memory from SHM buffer if available. - if (publisher_data->context->impl->shm_provider.has_value()) { - // printf(">>> rmw_publish(), SHM enabled\n"); + if (publisher_data->context->impl->shm.has_value() && + publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) { + // printf(">>> rmw_publish(), Will use SHM\n"); - const auto& provider = publisher_data->context->impl->shm_provider.value(); + const auto& provider = publisher_data->context->impl->shm.value().shm_provider; // Allocate SHM bufer // We use 1-byte alignment @@ -922,7 +923,7 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, } else #endif { - // printf(">>> rmw_publish(), SHM disabled\n"); + // printf(">>> rmw_publish(), Will use RAW\n"); // Get memory from the allocator. msg_bytes = static_cast( From 991a1a54ea6f8cd75dfa42d0081d2ae89e2c0742 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 28 Aug 2024 16:37:01 +0300 Subject: [PATCH 34/41] - refactor buffer serialization code - update default configs to have SHM enabled by default (if compiled with SHM support) --- .../DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 | 2 +- .../DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 | 2 +- rmw_zenoh_cpp/src/rmw_init.cpp | 14 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 270 +++++++++++------- 4 files changed, 168 insertions(+), 120 deletions(-) diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 index 0ffdf2bb..ffb16dd0 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 @@ -229,7 +229,7 @@ }, /// Shared memory configuration shared_memory: { - enabled: false, + enabled: true, }, /// Access control configuration auth: { diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 index b111bfdf..6b7adc36 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 @@ -232,7 +232,7 @@ }, /// Shared memory configuration shared_memory: { - enabled: false, + enabled: true, }, /// Access control configuration auth: { diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 086cf9eb..8c37be2f 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -175,12 +175,10 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { } #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - z_owned_string_t shm_enabled; - zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled); - auto free_shm_= rcpputils::make_scope_exit( - [&shm_enabled]() { - z_drop(z_move(shm_enabled)); - }); + z_owned_string_t shm_enabled_str; + zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled_str); + const bool shm_enabled = (strncmp(z_string_data(z_loan(shm_enabled_str)), "true", z_string_len(z_loan(shm_enabled_str))) == 0); + z_drop(z_move(shm_enabled_str)); #endif // Initialize the zenoh session. @@ -221,7 +219,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // Initialize the shm manager if shared_memory is enabled in the config. - if (strncmp(z_string_data(z_loan(shm_enabled)), "true", z_string_len(z_loan(shm_enabled))) == 0) { + if (shm_enabled) { printf(">>> SHM is enabled\n"); rmw_context_impl_s::rmw_shm_s shm; @@ -248,6 +246,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { // Upon successful provider creation, store it in the context context->impl->shm = std::make_optional(shm); + } else { + printf(">>> SHM is disabled\n"); } auto free_shm_provider = rcpputils::make_scope_exit( diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 18851c41..d5ad07c6 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -839,6 +839,151 @@ create_map_and_set_sequence_num(z_owned_bytes_t* out_bytes, } } // namespace +size_t serialize_into( + char *buffer, + size_t size, + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + const void *ros_message) { + // Object that manages the raw buffer + eprosima::fastcdr::FastBuffer fastbuffer(buffer, size); + + // Object that serializes the data + rmw_zenoh_cpp::Cdr ser(fastbuffer); + + // Serialize message + if (!publisher_data->type_support->serialize_ros_message( + ros_message, ser.get_cdr(), publisher_data->type_support_impl)) { + RMW_SET_ERROR_MSG("could not serialize ROS message"); + return 0; + } + + return ser.get_serialized_data_length(); +} + +rmw_ret_t publish( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + z_moved_bytes_t* payload) { + + auto free_payload = rcpputils::make_scope_exit( + [&payload]() { z_drop(payload); }); + + z_owned_bytes_t attachment; + if (!create_map_and_set_sequence_num( + &attachment, + publisher_data->get_next_sequence_number(), + publisher_data->pub_gid)) { + // create_map_and_set_sequence_num already set the error + return RMW_RET_ERROR; + } + + // 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); + options.attachment = z_move(attachment); + + free_payload.cancel(); + + if (z_publisher_put(z_loan(publisher_data->pub), payload, &options) != Z_OK) { + RMW_SET_ERROR_MSG("unable to publish message"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} + +/// Publish as RAW message. +rmw_ret_t publish_raw( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + const void *ros_message, + size_t max_data_length) { + // printf(">>> rmw_publish(), Will use RAW\n"); + + rcutils_allocator_t *allocator = + &(publisher_data->context->options.allocator); + + // Get memory from the allocator. + 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]() { + if (msg_bytes) { + allocator->deallocate(msg_bytes, allocator->state); + } + }); + + // Serialize message into memory + const size_t data_length = serialize_into(msg_bytes, max_data_length, publisher_data, ros_message); + // Return error upon unsuccessful serialization + if(data_length == 0) { + // serialize_into already set the error + return RMW_RET_ERROR; + } + + z_owned_bytes_t payload; + if (z_bytes_serialize_from_buf( + &payload, reinterpret_cast(msg_bytes), data_length) != Z_OK) { + RMW_SET_ERROR_MSG("unable to serialize raw buffer into Zenoh Payload"); + return RMW_RET_ERROR; + } + + return publish(publisher_data, z_move(payload)); +} + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +/// Publish as SHM message. +rmw_ret_t publish_shm( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + const void *ros_message, + size_t max_data_length, + const z_loaned_shm_provider_t *provider) { + // printf(">>> rmw_publish(), Will use SHM\n"); + + // Allocate SHM bufer + // We use 1-byte alignment + z_buf_layout_alloc_result_t alloc; + { + z_alloc_alignment_t alignment = {0}; + z_shm_provider_alloc_gc_defrag_blocking( + &alloc, provider, + max_data_length, alignment); + } + + // Check allocation status + if (alloc.status != ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Unexpected failure during SHM buffer allocation."); + return RMW_RET_ERROR; + } + + // Cleanup SHM buffer upon scope exit + auto always_free_shmbuf = rcpputils::make_scope_exit([&alloc]() { + z_drop(z_move(alloc.buf)); + }); + + // Serialize message into memory + char* msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); + const size_t data_length = serialize_into(msg_bytes, max_data_length, publisher_data, ros_message); + // Return error upon unsuccessful serialization + if(data_length == 0) { + // serialize_into already set the error + return RMW_RET_ERROR; + } + + z_owned_bytes_t payload; + if (z_bytes_serialize_from_shm_mut(&payload, z_move(alloc.buf)) != Z_OK) { + RMW_SET_ERROR_MSG("unable to serialize SHM buffer into Zenoh Payload"); + return RMW_RET_ERROR; + } + + return publish(publisher_data, z_move(payload)); +} +#endif + //============================================================================== /// Publish a ROS message. rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, @@ -858,137 +1003,40 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, static_cast(publisher->data); RMW_CHECK_FOR_NULL_WITH_MSG(publisher_data, "publisher_data is null", return RMW_RET_INVALID_ARGUMENT); - - rcutils_allocator_t *allocator = - &(publisher_data->context->options.allocator); - // Serialize data. size_t max_data_length = publisher_data->type_support->get_estimated_serialized_size( ros_message, publisher_data->type_support_impl); - // To store serialized message byte array. - char *msg_bytes = nullptr; - #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - std::optional shmbuf = std::nullopt; - auto always_free_shmbuf = rcpputils::make_scope_exit([&shmbuf]() { - if (shmbuf.has_value()) { - z_drop(z_move(shmbuf.value())); - } - }); -#endif - - auto free_msg_bytes = - rcpputils::make_scope_exit([&msg_bytes, allocator -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - , &shmbuf -#endif - ]() { - if (msg_bytes -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - && !shmbuf.has_value() -#endif - ) { - allocator->deallocate(msg_bytes, allocator->state); - } - }); - -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - // Get memory from SHM buffer if available. if (publisher_data->context->impl->shm.has_value() && publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) { - // printf(">>> rmw_publish(), Will use SHM\n"); - - const auto& provider = publisher_data->context->impl->shm.value().shm_provider; - - // Allocate SHM bufer - // We use 1-byte alignment - z_buf_layout_alloc_result_t alloc; - z_alloc_alignment_t alignment = {0}; - z_shm_provider_alloc_gc_defrag_blocking( - &alloc, z_loan(provider), - max_data_length, alignment); - - if (alloc.status == ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { - shmbuf = std::make_optional(alloc.buf); - msg_bytes = - reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); - } else { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Unexpected failure during SHM buffer allocation."); - return RMW_RET_ERROR; - } - - } else + return publish_shm( + publisher_data, + ros_message, + max_data_length, + z_loan(publisher_data->context->impl->shm.value().shm_provider)); + } else #endif - { - // printf(">>> rmw_publish(), Will use RAW\n"); + { + return publish_raw( + publisher_data, + ros_message, + max_data_length); + } +} + - // 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); - } - // Object that manages the raw buffer - eprosima::fastcdr::FastBuffer fastbuffer(msg_bytes, max_data_length); - // Object that serializes the data - rmw_zenoh_cpp::Cdr ser(fastbuffer); - if (!publisher_data->type_support->serialize_ros_message( - ros_message, ser.get_cdr(), publisher_data->type_support_impl)) { - RMW_SET_ERROR_MSG("could not serialize ROS message"); - return RMW_RET_ERROR; - } - const size_t data_length = ser.get_serialized_data_length(); - int64_t sequence_number = publisher_data->get_next_sequence_number(); - z_owned_bytes_t attachment; - if (!create_map_and_set_sequence_num(&attachment, sequence_number, publisher_data->pub_gid)) { - // create_map_and_set_sequence_num already set the error - return RMW_RET_ERROR; - } - auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() { z_drop(z_move(attachment)); }); - z_owned_bytes_t payload; -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - if (shmbuf.has_value()) { - if (z_bytes_serialize_from_shm_mut(&payload, z_move(shmbuf.value())) != Z_OK) { - RMW_SET_ERROR_MSG("unable to serialize SHM buffer into Zenoh Payload"); - return RMW_RET_ERROR; - } - } else -#endif - { - if (z_bytes_serialize_from_buf( - &payload, reinterpret_cast(msg_bytes), data_length) != Z_OK) { - RMW_SET_ERROR_MSG("unable to serialize raw buffer into Zenoh Payload"); - return RMW_RET_ERROR; - } - } - // we cancel free attachment because attachment will be moved into z_publisher_put_options_t - free_attachment.cancel(); - // 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); - options.attachment = z_move(attachment); - if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options) != Z_OK) { - RMW_SET_ERROR_MSG("unable to publish message"); - return RMW_RET_ERROR; - } - return RMW_RET_OK; -} //============================================================================== /// Publish a loaned ROS message. From a0d7d02714527be671b426dd33afbe33cd17ed3a Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 5 Sep 2024 21:06:30 +0300 Subject: [PATCH 35/41] wip on publication refactoring --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 192 +++++++++++++++++++------------- 1 file changed, 115 insertions(+), 77 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index d5ad07c6..03e6616a 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -839,26 +839,47 @@ create_map_and_set_sequence_num(z_owned_bytes_t* out_bytes, } } // namespace -size_t serialize_into( - char *buffer, - size_t size, - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - const void *ros_message) { - // Object that manages the raw buffer - eprosima::fastcdr::FastBuffer fastbuffer(buffer, size); +struct RosMsgSerializer { + static size_t serialize_into( + char *buffer, + size_t size, + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + const void *ros_message) { + // Object that manages the raw buffer + eprosima::fastcdr::FastBuffer fastbuffer(buffer, size); + + // Object that serializes the data + rmw_zenoh_cpp::Cdr ser(fastbuffer); + + // Serialize message + if (!publisher_data->type_support->serialize_ros_message( + ros_message, ser.get_cdr(), publisher_data->type_support_impl)) { + RMW_SET_ERROR_MSG("could not serialize ROS message"); + return 0; + } - // Object that serializes the data - rmw_zenoh_cpp::Cdr ser(fastbuffer); - - // Serialize message - if (!publisher_data->type_support->serialize_ros_message( - ros_message, ser.get_cdr(), publisher_data->type_support_impl)) { - RMW_SET_ERROR_MSG("could not serialize ROS message"); - return 0; + return ser.get_serialized_data_length(); } +}; - return ser.get_serialized_data_length(); -} + +struct RosSerializedMsgSerializer { + static size_t serialize_into( + char *buffer, + size_t size, + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data + const rmw_serialized_message_t *serialized_message,) { + eprosima::fastcdr::FastBuffer buffer( + reinterpret_cast(serialized_message->buffer), + serialized_message->buffer_length); + rmw_zenoh_cpp::Cdr ser(buffer); + if (!ser.get_cdr().jump(serialized_message->buffer_length)) { + RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); + return RMW_RET_ERROR; + } + return ser.get_serialized_data_length(); + } +}; rmw_ret_t publish( rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, @@ -893,11 +914,36 @@ rmw_ret_t publish( return RMW_RET_OK; } +rmw_2_zenoh_payload alloc_payload( +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + rmw_zenoh_cpp::rmw_publisher_data_t& publisher_data, +#endif + size_t length) { + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + if (publisher_data.context->impl->shm.has_value() && + publisher_data.context->impl->shm.value().msgsize_threshold <= max_data_length) { + return publish_shm( + publisher_data, + ros_message, + max_data_length, + z_loan(publisher_data->context->impl->shm.value().shm_provider)); + } else +#endif + { + return publish_raw( + publisher_data, + ros_message, + max_data_length); + } +} + /// Publish as RAW message. +template rmw_ret_t publish_raw( rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - const void *ros_message, - size_t max_data_length) { + size_t max_data_length, + SerializerArgs... serializer_args) { // printf(">>> rmw_publish(), Will use RAW\n"); rcutils_allocator_t *allocator = @@ -917,7 +963,11 @@ rmw_ret_t publish_raw( }); // Serialize message into memory - const size_t data_length = serialize_into(msg_bytes, max_data_length, publisher_data, ros_message); + const size_t data_length = Tserializer::serialize_into( + msg_bytes, + max_data_length, + publisher_data, + serializer_args...); // Return error upon unsuccessful serialization if(data_length == 0) { // serialize_into already set the error @@ -936,11 +986,12 @@ rmw_ret_t publish_raw( #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY /// Publish as SHM message. +template rmw_ret_t publish_shm( rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - const void *ros_message, size_t max_data_length, - const z_loaned_shm_provider_t *provider) { + const z_loaned_shm_provider_t *provider, + SerializerArgs... serializer_args) { // printf(">>> rmw_publish(), Will use SHM\n"); // Allocate SHM bufer @@ -967,23 +1018,55 @@ rmw_ret_t publish_shm( // Serialize message into memory char* msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); - const size_t data_length = serialize_into(msg_bytes, max_data_length, publisher_data, ros_message); + const size_t data_length = Tserializer::serialize_into( + msg_bytes, + max_data_length, + publisher_data, + serializer_args...); + // Return error upon unsuccessful serialization if(data_length == 0) { // serialize_into already set the error return RMW_RET_ERROR; } + // construct z_owned_bytes_t from SHM buffer z_owned_bytes_t payload; if (z_bytes_serialize_from_shm_mut(&payload, z_move(alloc.buf)) != Z_OK) { RMW_SET_ERROR_MSG("unable to serialize SHM buffer into Zenoh Payload"); return RMW_RET_ERROR; } + // publish data return publish(publisher_data, z_move(payload)); } #endif +/// Publish using raw or SHM(if applicable) buffer +template +rmw_ret_t publish_with_method_selection( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + SerializerArgs... serializer_args) { + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + if (publisher_data->context->impl->shm.has_value() && + publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) { + return publish_shm( + publisher_data, + max_data_length, + z_loan(publisher_data->context->impl->shm.value().shm_provider), + serializer_args...); + } else +#endif + { + return publish_raw( + publisher_data, + max_data_length, + serializer_args...); + } +} + //============================================================================== /// Publish a ROS message. rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, @@ -1003,27 +1086,15 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, static_cast(publisher->data); RMW_CHECK_FOR_NULL_WITH_MSG(publisher_data, "publisher_data is null", return RMW_RET_INVALID_ARGUMENT); - // Serialize data. + // estimate serialized data size size_t max_data_length = publisher_data->type_support->get_estimated_serialized_size( ros_message, publisher_data->type_support_impl); -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - if (publisher_data->context->impl->shm.has_value() && - publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) { - return publish_shm( - publisher_data, - ros_message, - max_data_length, - z_loan(publisher_data->context->impl->shm.value().shm_provider)); - } else -#endif - { - return publish_raw( - publisher_data, - ros_message, - max_data_length); - } + return publish_with_method_selection( + publisher_data, + max_data_length, + ros_message); } @@ -1115,43 +1186,10 @@ rmw_ret_t rmw_publish_serialized_message( 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); - rmw_zenoh_cpp::Cdr ser(buffer); - if (!ser.get_cdr().jump(serialized_message->buffer_length)) { - RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); - return RMW_RET_ERROR; - } - - uint64_t sequence_number = publisher_data->get_next_sequence_number(); - - z_owned_bytes_t attachment; - if (!create_map_and_set_sequence_num(&attachment, sequence_number, publisher_data->pub_gid)) { - // create_map_and_set_sequence_num already set the error - return RMW_RET_ERROR; - } - auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() { z_drop(z_move(attachment)); }); - - const size_t data_length = ser.get_serialized_data_length(); - - // 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); - options.attachment = z_move(attachment); - - z_owned_bytes_t payload; - z_bytes_serialize_from_buf(&payload, serialized_message->buffer, data_length); - - if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options)) { - RMW_SET_ERROR_MSG("unable to publish message"); - return RMW_RET_ERROR; - } - - return RMW_RET_OK; + return publish_with_method_selection( + publisher_data, + serialized_message->buffer_length, + serialized_message); } //============================================================================== From 2f8b788627e413cf6c4eb47c7a1eb4db42c48646 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 11 Sep 2024 11:59:30 +0300 Subject: [PATCH 36/41] WIP on SHM (benchmarked) --- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 2 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 404 ++++++++++------------ zenoh_c_vendor/CMakeLists.txt | 27 +- 3 files changed, 195 insertions(+), 238 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 4f4bf4a5..6e8a3c7e 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -44,7 +44,7 @@ static const std::unordered_map(now); + int64_t source_timestamp = now_ns.count(); + + rmw_zenoh_cpp::attachement_data_t data(sequence_number, source_timestamp, + gid); + if (data.serialize_to_zbytes(out_bytes)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Failed to serialize the attachment"); + return false; + } + + return true; +} + +rmw_ret_t publish( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + z_moved_bytes_t* payload) { + + auto free_payload = rcpputils::make_scope_exit( + [&payload]() { z_drop(payload); }); + + z_owned_bytes_t attachment; + if (!create_map_and_set_sequence_num( + &attachment, + publisher_data->get_next_sequence_number(), + publisher_data->pub_gid)) { + // create_map_and_set_sequence_num already set the error + return RMW_RET_ERROR; + } + + // 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); + options.attachment = z_move(attachment); + + free_payload.cancel(); + + if (z_publisher_put(z_loan(publisher_data->pub), payload, &options) != Z_OK) { + RMW_SET_ERROR_MSG("unable to publish message"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} + +/// Publish as RAW message. +template +rmw_ret_t publish_raw( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + SerializerArgs... serializer_args) { + // printf(">>> rmw_publish(), Will use RAW\n"); + + rcutils_allocator_t *allocator = + &(publisher_data->context->options.allocator); + + // Get memory from the allocator. + 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]() { + if (msg_bytes) { + allocator->deallocate(msg_bytes, allocator->state); + } + }); + + // Serialize message into memory + const size_t data_length = Tserializer::serialize_into( + msg_bytes, + max_data_length, + publisher_data, + serializer_args...); + // Return error upon unsuccessful serialization + if(data_length == 0) { + // serialize_into already set the error + return RMW_RET_ERROR; + } + + z_owned_bytes_t payload; + if (z_bytes_serialize_from_buf( + &payload, reinterpret_cast(msg_bytes), data_length) != Z_OK) { + RMW_SET_ERROR_MSG("unable to serialize raw buffer into Zenoh Payload"); + return RMW_RET_ERROR; + } + + return publish(publisher_data, z_move(payload)); +} + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +/// Publish as SHM message. +template +rmw_ret_t publish_shm( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + const z_loaned_shm_provider_t *provider, + SerializerArgs... serializer_args) { + // printf(">>> rmw_publish(), Will use SHM\n"); + + // Allocate SHM bufer + // We use 1-byte alignment + z_buf_layout_alloc_result_t alloc; + { + z_alloc_alignment_t alignment = {0}; + z_shm_provider_alloc_gc_defrag_blocking( + &alloc, provider, + max_data_length, alignment); + } + + // Check allocation status + if (alloc.status != ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Unexpected failure during SHM buffer allocation."); + return RMW_RET_ERROR; + } + + // Cleanup SHM buffer upon scope exit + auto always_free_shmbuf = rcpputils::make_scope_exit([&alloc]() { + z_drop(z_move(alloc.buf)); + }); + + // Serialize message into memory + char* msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); + const size_t data_length = Tserializer::serialize_into( + msg_bytes, + max_data_length, + publisher_data, + serializer_args...); + + // Return error upon unsuccessful serialization + if(data_length == 0) { + // serialize_into already set the error + return RMW_RET_ERROR; + } + + // construct z_owned_bytes_t from SHM buffer + z_owned_bytes_t payload; + if (z_bytes_serialize_from_shm_mut(&payload, z_move(alloc.buf)) != Z_OK) { + RMW_SET_ERROR_MSG("unable to serialize SHM buffer into Zenoh Payload"); + return RMW_RET_ERROR; + } + + // publish data + return publish(publisher_data, z_move(payload)); +} +#endif + +/// Publish using raw or SHM(if applicable) buffer +template +rmw_ret_t publish_with_method_selection( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + SerializerArgs... serializer_args) { + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + if (publisher_data->context->impl->shm.has_value() && + publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) { + return publish_shm( + publisher_data, + max_data_length, + z_loan(publisher_data->context->impl->shm.value().shm_provider), + serializer_args...); + } else +#endif + { + return publish_raw( + publisher_data, + max_data_length, + serializer_args...); + } +} + } // namespace extern "C" { @@ -806,31 +991,6 @@ rmw_return_loaned_message_from_publisher( return RMW_RET_UNSUPPORTED; } -namespace -{ -bool -create_map_and_set_sequence_num( - z_owned_bytes_t * out_bytes, - int64_t sequence_number, - uint8_t gid[RMW_GID_STORAGE_SIZE]) -{ - auto now = std::chrono::system_clock::now().time_since_epoch(); - auto now_ns = std::chrono::duration_cast(now); - int64_t source_timestamp = now_ns.count(); - - rmw_zenoh_cpp::attachement_data_t data(sequence_number, source_timestamp, - gid); - if (data.serialize_to_zbytes(out_bytes)) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Failed to serialize the attachment"); - return false; - } - - return true; -} -} // namespace - struct RosMsgSerializer { static size_t serialize_into( char *buffer, @@ -857,10 +1017,10 @@ struct RosMsgSerializer { struct RosSerializedMsgSerializer { static size_t serialize_into( - char *buffer, - size_t size, - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data - const rmw_serialized_message_t *serialized_message,) { + char *_buffer, + size_t _size, + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + const rmw_serialized_message_t *serialized_message) { eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); @@ -873,192 +1033,6 @@ struct RosSerializedMsgSerializer { } }; -rmw_ret_t publish( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - z_moved_bytes_t* payload) { - - auto free_payload = rcpputils::make_scope_exit( - [&payload]() { z_drop(payload); }); - - z_owned_bytes_t attachment; - if (!create_map_and_set_sequence_num( - &attachment, - publisher_data->get_next_sequence_number(), - publisher_data->pub_gid)) { - // create_map_and_set_sequence_num already set the error - return RMW_RET_ERROR; - } - - // 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); - options.attachment = z_move(attachment); - - free_payload.cancel(); - - if (z_publisher_put(z_loan(publisher_data->pub), payload, &options) != Z_OK) { - RMW_SET_ERROR_MSG("unable to publish message"); - return RMW_RET_ERROR; - } - - return RMW_RET_OK; -} - -rmw_2_zenoh_payload alloc_payload( -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - rmw_zenoh_cpp::rmw_publisher_data_t& publisher_data, -#endif - size_t length) { - -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - if (publisher_data.context->impl->shm.has_value() && - publisher_data.context->impl->shm.value().msgsize_threshold <= max_data_length) { - return publish_shm( - publisher_data, - ros_message, - max_data_length, - z_loan(publisher_data->context->impl->shm.value().shm_provider)); - } else -#endif - { - return publish_raw( - publisher_data, - ros_message, - max_data_length); - } -} - -/// Publish as RAW message. -template -rmw_ret_t publish_raw( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - size_t max_data_length, - SerializerArgs... serializer_args) { - // printf(">>> rmw_publish(), Will use RAW\n"); - - rcutils_allocator_t *allocator = - &(publisher_data->context->options.allocator); - - // Get memory from the allocator. - 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]() { - if (msg_bytes) { - allocator->deallocate(msg_bytes, allocator->state); - } - }); - - // Serialize message into memory - const size_t data_length = Tserializer::serialize_into( - msg_bytes, - max_data_length, - publisher_data, - serializer_args...); - // Return error upon unsuccessful serialization - if(data_length == 0) { - // serialize_into already set the error - return RMW_RET_ERROR; - } - - z_owned_bytes_t payload; - if (z_bytes_serialize_from_buf( - &payload, reinterpret_cast(msg_bytes), data_length) != Z_OK) { - RMW_SET_ERROR_MSG("unable to serialize raw buffer into Zenoh Payload"); - return RMW_RET_ERROR; - } - - return publish(publisher_data, z_move(payload)); -} - -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY -/// Publish as SHM message. -template -rmw_ret_t publish_shm( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - size_t max_data_length, - const z_loaned_shm_provider_t *provider, - SerializerArgs... serializer_args) { - // printf(">>> rmw_publish(), Will use SHM\n"); - - // Allocate SHM bufer - // We use 1-byte alignment - z_buf_layout_alloc_result_t alloc; - { - z_alloc_alignment_t alignment = {0}; - z_shm_provider_alloc_gc_defrag_blocking( - &alloc, provider, - max_data_length, alignment); - } - - // Check allocation status - if (alloc.status != ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Unexpected failure during SHM buffer allocation."); - return RMW_RET_ERROR; - } - - // Cleanup SHM buffer upon scope exit - auto always_free_shmbuf = rcpputils::make_scope_exit([&alloc]() { - z_drop(z_move(alloc.buf)); - }); - - // Serialize message into memory - char* msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); - const size_t data_length = Tserializer::serialize_into( - msg_bytes, - max_data_length, - publisher_data, - serializer_args...); - - // Return error upon unsuccessful serialization - if(data_length == 0) { - // serialize_into already set the error - return RMW_RET_ERROR; - } - - // construct z_owned_bytes_t from SHM buffer - z_owned_bytes_t payload; - if (z_bytes_serialize_from_shm_mut(&payload, z_move(alloc.buf)) != Z_OK) { - RMW_SET_ERROR_MSG("unable to serialize SHM buffer into Zenoh Payload"); - return RMW_RET_ERROR; - } - - // publish data - return publish(publisher_data, z_move(payload)); -} -#endif - -/// Publish using raw or SHM(if applicable) buffer -template -rmw_ret_t publish_with_method_selection( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - size_t max_data_length, - SerializerArgs... serializer_args) { - -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - if (publisher_data->context->impl->shm.has_value() && - publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) { - return publish_shm( - publisher_data, - max_data_length, - z_loan(publisher_data->context->impl->shm.value().shm_provider), - serializer_args...); - } else -#endif - { - return publish_raw( - publisher_data, - max_data_length, - serializer_args...); - } -} - //============================================================================== /// Publish a ROS message. rmw_ret_t rmw_publish( diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 56a84305..ed457fa8 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() find_package(ament_cmake REQUIRED) -# find_package(ament_cmake_vendor_package REQUIRED) +find_package(ament_cmake_vendor_package REQUIRED) # Disable default features and enable only the most useful ones. This reduces build time and footprint. # For a complete list of features see: https://github.com/eclipse-zenoh/zenoh/blob/main/zenoh/Cargo.toml @@ -26,32 +26,15 @@ set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY OFF CACHE BOOL "Compile Zenoh RMW with Sh # - https://github.com/eclipse-zenoh/zenoh-c/pull/358 (fix debian packaging issue: https://github.com/jspricke/ros-deb-builder-action/issues/49) # - https://github.com/eclipse-zenoh/zenoh/pull/1150 (fix deadlock issue https://github.com/ros2/rmw_zenoh/issues/182) # - https://github.com/eclipse-zenoh/zenoh-c/pull/620 (fix ze_querying_subscriber_get API to query newly discovered publishers) -#ament_vendor(zenoh_c_vendor -# VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git -# VCS_VERSION 5d00adfd694bea55069b640ece71895ed182e61b -# CMAKE_ARGS -# "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" -# "-DZENOHC_BUILD_WITH_SHARED_MEMORY=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}" -# "-DZENOHC_BUILD_WITH_UNSTABLE_API=true" -# "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" -#) -include(ExternalProject) -set(ZENOHC_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}) -ExternalProject_Add(zenohc - PREFIX ${CMAKE_BINARY_DIR}/zenohc - GIT_REPOSITORY https://github.com/eclipse-zenoh/zenoh-c.git - GIT_TAG main +ament_vendor(zenoh_c_vendor + VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git + VCS_VERSION 5d00adfd694bea55069b640ece71895ed182e61b CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_SHARED_MEMORY=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}" - "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" + "-DZENOHC_BUILD_WITH_UNSTABLE_API=true" "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" - BUILD_IN_SOURCE 1 ) -add_custom_target(zenoh_c_vendor ALL - DEPENDS zenohc -) -list(APPEND CMAKE_PREFIX_PATH ${ZENOHC_INSTALL_DIR}) ament_export_dependencies(zenohc) From 1ce3cf8fbbe99306beee314f4e21ea6b4c8eeca0 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 25 Sep 2024 09:30:53 +0300 Subject: [PATCH 37/41] Update rmw_zenoh.cpp --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 401 +++++++++++++++++++------------- 1 file changed, 243 insertions(+), 158 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 76fff6b6..dff3464a 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -104,12 +104,194 @@ const rosidl_service_type_support_t * find_service_type_support( return type_support; } -} // namespace -extern "C" +bool +create_map_and_set_sequence_num( + z_owned_bytes_t * out_bytes, + int64_t sequence_number, + uint8_t gid[RMW_GID_STORAGE_SIZE]) { -// TODO(yuyuan): SHM, make this configurable -#define SHM_BUF_OK_SIZE 2621440 + auto now = std::chrono::system_clock::now().time_since_epoch(); + auto now_ns = std::chrono::duration_cast(now); + int64_t source_timestamp = now_ns.count(); + + rmw_zenoh_cpp::attachement_data_t data(sequence_number, source_timestamp, + gid); + if (data.serialize_to_zbytes(out_bytes)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Failed to serialize the attachment"); + return false; + } + + return true; +} + +rmw_ret_t publish( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + z_moved_bytes_t* payload) { + + auto free_payload = rcpputils::make_scope_exit( + [&payload]() { z_drop(payload); }); + + z_owned_bytes_t attachment; + if (!create_map_and_set_sequence_num( + &attachment, + publisher_data->get_next_sequence_number(), + publisher_data->pub_gid)) { + // create_map_and_set_sequence_num already set the error + return RMW_RET_ERROR; + } + + // 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); + options.attachment = z_move(attachment); + + free_payload.cancel(); + + if (z_publisher_put(z_loan(publisher_data->pub), payload, &options) != Z_OK) { + RMW_SET_ERROR_MSG("unable to publish message"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} + +/// Publish as RAW message. +template +rmw_ret_t publish_raw( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + SerializerArgs... serializer_args) { + // printf(">>> rmw_publish(), Will use RAW\n"); + + rcutils_allocator_t *allocator = + &(publisher_data->context->options.allocator); + + // Get memory from the allocator. + 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]() { + if (msg_bytes) { + allocator->deallocate(msg_bytes, allocator->state); + } + }); + + // Serialize message into memory + const size_t data_length = Tserializer::serialize_into( + msg_bytes, + max_data_length, + publisher_data, + serializer_args...); + // Return error upon unsuccessful serialization + if(data_length == 0) { + // serialize_into already set the error + return RMW_RET_ERROR; + } + + z_owned_bytes_t payload; + if (z_bytes_serialize_from_buf( + &payload, reinterpret_cast(msg_bytes), data_length) != Z_OK) { + RMW_SET_ERROR_MSG("unable to serialize raw buffer into Zenoh Payload"); + return RMW_RET_ERROR; + } + + return publish(publisher_data, z_move(payload)); +} + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +/// Publish as SHM message. +template +rmw_ret_t publish_shm( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + const z_loaned_shm_provider_t *provider, + SerializerArgs... serializer_args) { + // printf(">>> rmw_publish(), Will use SHM\n"); + + // Allocate SHM bufer + // We use 1-byte alignment + z_buf_layout_alloc_result_t alloc; + { + z_alloc_alignment_t alignment = {0}; + z_shm_provider_alloc_gc_defrag_blocking( + &alloc, provider, + max_data_length, alignment); + } + + // Check allocation status + if (alloc.status != ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Unexpected failure during SHM buffer allocation."); + return RMW_RET_ERROR; + } + + // Cleanup SHM buffer upon scope exit + auto always_free_shmbuf = rcpputils::make_scope_exit([&alloc]() { + z_drop(z_move(alloc.buf)); + }); + + // Serialize message into memory + char* msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); + const size_t data_length = Tserializer::serialize_into( + msg_bytes, + max_data_length, + publisher_data, + serializer_args...); + + // Return error upon unsuccessful serialization + if(data_length == 0) { + // serialize_into already set the error + return RMW_RET_ERROR; + } + + // construct z_owned_bytes_t from SHM buffer + z_owned_bytes_t payload; + if (z_bytes_serialize_from_shm_mut(&payload, z_move(alloc.buf)) != Z_OK) { + RMW_SET_ERROR_MSG("unable to serialize SHM buffer into Zenoh Payload"); + return RMW_RET_ERROR; + } + + // publish data + return publish(publisher_data, z_move(payload)); +} +#endif + +/// Publish using raw or SHM(if applicable) buffer +template +rmw_ret_t publish_with_method_selection( + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + SerializerArgs... serializer_args) { + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + if (publisher_data->context->impl->shm.has_value() && + publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) { + return publish_shm( + publisher_data, + max_data_length, + z_loan(publisher_data->context->impl->shm.value().shm_provider), + serializer_args...); + } else +#endif + { + return publish_raw( + publisher_data, + max_data_length, + serializer_args...); + } +} + +} // namespace + +extern "C" { //============================================================================== /// Get the name of the rmw implementation being used @@ -798,29 +980,47 @@ rmw_return_loaned_message_from_publisher( return RMW_RET_UNSUPPORTED; } -namespace -{ -bool -create_map_and_set_sequence_num( - z_owned_bytes_t * out_bytes, - int64_t sequence_number, - uint8_t gid[RMW_GID_STORAGE_SIZE]) -{ - auto now = std::chrono::system_clock::now().time_since_epoch(); - auto now_ns = std::chrono::duration_cast(now); - int64_t source_timestamp = now_ns.count(); +struct RosMsgSerializer { + static size_t serialize_into( + char *buffer, + size_t size, + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + const void *ros_message) { + // Object that manages the raw buffer + eprosima::fastcdr::FastBuffer fastbuffer(buffer, size); + + // Object that serializes the data + rmw_zenoh_cpp::Cdr ser(fastbuffer); + + // Serialize message + if (!publisher_data->type_support->serialize_ros_message( + ros_message, ser.get_cdr(), publisher_data->type_support_impl)) { + RMW_SET_ERROR_MSG("could not serialize ROS message"); + return 0; + } - rmw_zenoh_cpp::attachement_data_t data(sequence_number, source_timestamp, gid); - if (data.serialize_to_zbytes(out_bytes)) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Failed to serialize the attachment"); - return false; + return ser.get_serialized_data_length(); } +}; - return true; -} -} // namespace + +struct RosSerializedMsgSerializer { + static size_t serialize_into( + char *_buffer, + size_t _size, + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + const rmw_serialized_message_t *serialized_message) { + eprosima::fastcdr::FastBuffer buffer( + reinterpret_cast(serialized_message->buffer), + serialized_message->buffer_length); + rmw_zenoh_cpp::Cdr ser(buffer); + if (!ser.get_cdr().jump(serialized_message->buffer_length)) { + RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); + return RMW_RET_ERROR; + } + return ser.get_serialized_data_length(); + } +}; //============================================================================== /// Publish a ROS message. @@ -841,111 +1041,31 @@ rmw_publish( 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); + auto publisher_data = + static_cast(publisher->data); + RMW_CHECK_FOR_NULL_WITH_MSG(publisher_data, "publisher_data is null", + return RMW_RET_INVALID_ARGUMENT); + // estimate serialized data size + size_t max_data_length = + publisher_data->type_support->get_estimated_serialized_size( + ros_message, publisher_data->type_support_impl); + + return publish_with_method_selection( + publisher_data, + max_data_length, + ros_message); +} - rcutils_allocator_t * allocator = &(publisher_data->context->options.allocator); - // Serialize data. - size_t max_data_length = publisher_data->type_support->get_estimated_serialized_size( - ros_message, - publisher_data->type_support_impl); - // To store serialized message byte array. - char * msg_bytes = nullptr; - std::optional shmbuf = std::nullopt; - auto always_free_shmbuf = rcpputils::make_scope_exit( - [&shmbuf]() { - if (shmbuf.has_value()) { - z_drop(z_move(shmbuf.value())); - } - }); - auto free_msg_bytes = rcpputils::make_scope_exit( - [&msg_bytes, allocator, &shmbuf]() { - if (msg_bytes && !shmbuf.has_value()) { - allocator->deallocate(msg_bytes, allocator->state); - } - }); - // Get memory from SHM buffer if available. - if (publisher_data->context->impl->shm_provider.has_value()) { - RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled."); - auto provider = publisher_data->context->impl->shm_provider.value(); - z_buf_layout_alloc_result_t alloc; - // TODO(yuyuan): SHM, configure this - z_alloc_alignment_t alignment = {5}; - z_shm_provider_alloc_gc_defrag_blocking(&alloc, z_loan(provider), SHM_BUF_OK_SIZE, alignment); - if (alloc.status == ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { - shmbuf = std::make_optional(alloc.buf); - msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); - } else { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unexpected failure during SHM buffer allocation."); - return RMW_RET_ERROR; - } - } 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); - } - // Object that manages the raw buffer - eprosima::fastcdr::FastBuffer fastbuffer(msg_bytes, max_data_length); - // Object that serializes the data - rmw_zenoh_cpp::Cdr ser(fastbuffer); - if (!publisher_data->type_support->serialize_ros_message( - ros_message, - ser.get_cdr(), - publisher_data->type_support_impl)) - { - RMW_SET_ERROR_MSG("could not serialize ROS message"); - return RMW_RET_ERROR; - } - const size_t data_length = ser.get_serialized_data_length(); - int64_t sequence_number = publisher_data->get_next_sequence_number(); - - z_owned_bytes_t attachment; - if (!create_map_and_set_sequence_num(&attachment, sequence_number, publisher_data->pub_gid)) { - // create_map_and_set_sequence_num already set the error - return RMW_RET_ERROR; - } - auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() { - z_drop(z_move(attachment)); - }); - - // 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); - options.attachment = z_move(attachment); - - z_owned_bytes_t payload; - - if (shmbuf.has_value()) { - z_bytes_serialize_from_shm_mut(&payload, z_move(shmbuf.value())); - } else { - z_bytes_serialize_from_buf(&payload, reinterpret_cast(msg_bytes), data_length); - } - - if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options) != Z_OK) { - RMW_SET_ERROR_MSG("unable to publish message"); - return RMW_RET_ERROR; - } - - return RMW_RET_OK; -} //============================================================================== /// Publish a loaned ROS message. @@ -1035,45 +1155,10 @@ rmw_publish_serialized_message( publisher_data, "publisher data pointer is null", return RMW_RET_ERROR); - eprosima::fastcdr::FastBuffer buffer( - reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); - rmw_zenoh_cpp::Cdr ser(buffer); - if (!ser.get_cdr().jump(serialized_message->buffer_length)) { - RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); - return RMW_RET_ERROR; - } - - uint64_t sequence_number = publisher_data->get_next_sequence_number(); - - z_owned_bytes_t attachment; - if (!create_map_and_set_sequence_num(&attachment, sequence_number, publisher_data->pub_gid)) { - // create_map_and_set_sequence_num already set the error - return RMW_RET_ERROR; - } - auto free_attachment = - rcpputils::make_scope_exit( - [&attachment]() { - z_drop(z_move(attachment)); - }); - - const size_t data_length = ser.get_serialized_data_length(); - - // 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); - options.attachment = z_move(attachment); - - z_owned_bytes_t payload; - z_bytes_serialize_from_buf(&payload, serialized_message->buffer, data_length); - - if (z_publisher_put(z_loan(publisher_data->pub), z_move(payload), &options) != Z_OK) { - RMW_SET_ERROR_MSG("unable to publish message"); - return RMW_RET_ERROR; - } - - return RMW_RET_OK; + return publish_with_method_selection( + publisher_data, + serialized_message->buffer_length, + serialized_message); } //============================================================================== @@ -3948,4 +4033,4 @@ rmw_client_set_on_new_response_callback( user_data, callback); return RMW_RET_OK; } -} // extern "C" \ No newline at end of file +} // extern "C" From 01b34c544dfe81ac81f5be3e7bfdba02e00136ed Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 25 Sep 2024 10:36:50 +0300 Subject: [PATCH 38/41] codestyle --- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 3 +- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 12 +- rmw_zenoh_cpp/src/rmw_init.cpp | 11 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 168 ++++++++++---------- 4 files changed, 101 insertions(+), 93 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 50d46afc..a28ce372 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -53,7 +53,8 @@ class rmw_context_impl_s final z_owned_session_t session; #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - struct rmw_shm_s { + struct rmw_shm_s + { z_owned_shm_provider_t shm_provider; size_t msgsize_threshold; }; diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 6e8a3c7e..3ce2ecc3 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -44,9 +44,9 @@ static const std::unordered_map zenoh_router_check_attempts() #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY ///============================================================================= -size_t zenoh_shm_alloc_size() { +size_t zenoh_shm_alloc_size() +{ const char * envar_value; if (NULL != rcutils_get_env(zenoh_shm_alloc_size_envar, &envar_value)) { @@ -142,7 +143,7 @@ size_t zenoh_shm_alloc_size() { zenoh_shm_alloc_size_envar); return zenoh_shm_alloc_size_default; } - + // If the environment variable contains a value, handle it accordingly. if (envar_value[0] != '\0') { const auto read_value = std::strtoull(envar_value, nullptr, 10); @@ -160,7 +161,8 @@ size_t zenoh_shm_alloc_size() { return zenoh_shm_alloc_size_default; } ///============================================================================= -size_t zenoh_shm_message_size_threshold() { +size_t zenoh_shm_message_size_threshold() +{ const char * envar_value; if (NULL != rcutils_get_env(zenoh_shm_message_size_threshold_envar, &envar_value)) { diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 04d984c2..1e964aec 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -190,7 +190,11 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY z_owned_string_t shm_enabled_str; zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled_str); - const bool shm_enabled = (strncmp(z_string_data(z_loan(shm_enabled_str)), "true", z_string_len(z_loan(shm_enabled_str))) == 0); + const bool shm_enabled = + (strncmp( + z_string_data(z_loan(shm_enabled_str)), "true", z_string_len( + z_loan( + shm_enabled_str))) == 0); z_drop(z_move(shm_enabled_str)); #endif @@ -244,13 +248,14 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // Provider's alignment will be 1 byte as we are going to make only 1-byte aligned allocations z_alloc_alignment_t alignment = {0}; z_owned_memory_layout_t layout; - if(z_memory_layout_new(&layout, rmw_zenoh_cpp::zenoh_shm_alloc_size(), alignment) != Z_OK) { + if (z_memory_layout_new(&layout, rmw_zenoh_cpp::zenoh_shm_alloc_size(), alignment) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a Layout for SHM provider."); return RMW_RET_ERROR; } // Create SHM provider - const auto provider_creation_result = z_posix_shm_provider_new(&shm.shm_provider, z_loan(layout)); + const auto provider_creation_result = + z_posix_shm_provider_new(&shm.shm_provider, z_loan(layout)); z_drop(z_move(layout)); if (provider_creation_result != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a SHM provider."); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index dff3464a..17fa015b 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -128,17 +128,17 @@ create_map_and_set_sequence_num( } rmw_ret_t publish( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - z_moved_bytes_t* payload) { + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + z_moved_bytes_t* payload) { auto free_payload = rcpputils::make_scope_exit( - [&payload]() { z_drop(payload); }); + [&payload]() { z_drop(payload); }); z_owned_bytes_t attachment; if (!create_map_and_set_sequence_num( - &attachment, - publisher_data->get_next_sequence_number(), - publisher_data->pub_gid)) { + &attachment, + publisher_data->get_next_sequence_number(), + publisher_data->pub_gid)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } @@ -161,44 +161,47 @@ rmw_ret_t publish( } /// Publish as RAW message. -template +template rmw_ret_t publish_raw( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - size_t max_data_length, - SerializerArgs... serializer_args) { + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + SerializerArgs... serializer_args) { // printf(">>> rmw_publish(), Will use RAW\n"); - rcutils_allocator_t *allocator = - &(publisher_data->context->options.allocator); + rcutils_allocator_t * allocator = + &(publisher_data->context->options.allocator); // Get memory from the allocator. 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); + 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]() { - if (msg_bytes) { - allocator->deallocate(msg_bytes, allocator->state); - } - }); + rcpputils::make_scope_exit( + [&msg_bytes, allocator]() { + if (msg_bytes) { + allocator->deallocate(msg_bytes, allocator->state); + } + }); // Serialize message into memory const size_t data_length = Tserializer::serialize_into( - msg_bytes, - max_data_length, - publisher_data, - serializer_args...); + msg_bytes, + max_data_length, + publisher_data, + serializer_args...); // Return error upon unsuccessful serialization - if(data_length == 0) { + if (data_length == 0) { // serialize_into already set the error return RMW_RET_ERROR; } z_owned_bytes_t payload; if (z_bytes_serialize_from_buf( - &payload, reinterpret_cast(msg_bytes), data_length) != Z_OK) { + &payload, reinterpret_cast(msg_bytes), data_length) != Z_OK) + { RMW_SET_ERROR_MSG("unable to serialize raw buffer into Zenoh Payload"); return RMW_RET_ERROR; } @@ -208,12 +211,12 @@ rmw_ret_t publish_raw( #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY /// Publish as SHM message. -template +template rmw_ret_t publish_shm( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - size_t max_data_length, - const z_loaned_shm_provider_t *provider, - SerializerArgs... serializer_args) { + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + const z_loaned_shm_provider_t *provider, + SerializerArgs... serializer_args) { // printf(">>> rmw_publish(), Will use SHM\n"); // Allocate SHM bufer @@ -222,32 +225,33 @@ rmw_ret_t publish_shm( { z_alloc_alignment_t alignment = {0}; z_shm_provider_alloc_gc_defrag_blocking( - &alloc, provider, - max_data_length, alignment); + &alloc, provider, + max_data_length, alignment); } // Check allocation status if (alloc.status != ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Unexpected failure during SHM buffer allocation."); + "rmw_zenoh_cpp", "Unexpected failure during SHM buffer allocation."); return RMW_RET_ERROR; } // Cleanup SHM buffer upon scope exit - auto always_free_shmbuf = rcpputils::make_scope_exit([&alloc]() { + auto always_free_shmbuf = rcpputils::make_scope_exit( + [&alloc]() { z_drop(z_move(alloc.buf)); - }); + }); // Serialize message into memory char* msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); const size_t data_length = Tserializer::serialize_into( - msg_bytes, - max_data_length, - publisher_data, - serializer_args...); + msg_bytes, + max_data_length, + publisher_data, + serializer_args...); // Return error upon unsuccessful serialization - if(data_length == 0) { + if (data_length == 0) { // serialize_into already set the error return RMW_RET_ERROR; } @@ -265,27 +269,28 @@ rmw_ret_t publish_shm( #endif /// Publish using raw or SHM(if applicable) buffer -template +template rmw_ret_t publish_with_method_selection( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - size_t max_data_length, - SerializerArgs... serializer_args) { - + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + size_t max_data_length, + SerializerArgs... serializer_args) +{ #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY if (publisher_data->context->impl->shm.has_value() && - publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) { - return publish_shm( - publisher_data, - max_data_length, - z_loan(publisher_data->context->impl->shm.value().shm_provider), - serializer_args...); - } else + publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) + { + return publish_shm( + publisher_data, + max_data_length, + z_loan(publisher_data->context->impl->shm.value().shm_provider), + serializer_args...); + } else #endif { return publish_raw( publisher_data, max_data_length, - serializer_args...); + serializer_args ...); } } @@ -980,12 +985,14 @@ rmw_return_loaned_message_from_publisher( return RMW_RET_UNSUPPORTED; } -struct RosMsgSerializer { +struct RosMsgSerializer +{ static size_t serialize_into( - char *buffer, - size_t size, - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - const void *ros_message) { + char *buffer, + size_t size, + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + const void *ros_message) + { // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(buffer, size); @@ -994,7 +1001,8 @@ struct RosMsgSerializer { // Serialize message if (!publisher_data->type_support->serialize_ros_message( - ros_message, ser.get_cdr(), publisher_data->type_support_impl)) { + ros_message, ser.get_cdr(), publisher_data->type_support_impl)) + { RMW_SET_ERROR_MSG("could not serialize ROS message"); return 0; } @@ -1004,15 +1012,17 @@ struct RosMsgSerializer { }; -struct RosSerializedMsgSerializer { +struct RosSerializedMsgSerializer +{ static size_t serialize_into( - char *_buffer, - size_t _size, - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - const rmw_serialized_message_t *serialized_message) { + char *_buffer, + size_t _size, + rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + const rmw_serialized_message_t *serialized_message) + { eprosima::fastcdr::FastBuffer buffer( - reinterpret_cast(serialized_message->buffer), - serialized_message->buffer_length); + reinterpret_cast(serialized_message->buffer), + serialized_message->buffer_length); rmw_zenoh_cpp::Cdr ser(buffer); if (!ser.get_cdr().jump(serialized_message->buffer_length)) { RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); @@ -1042,31 +1052,21 @@ rmw_publish( 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); + static_cast(publisher->data); + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher_data, "publisher_data is null", + return RMW_RET_INVALID_ARGUMENT); // estimate serialized data size size_t max_data_length = publisher_data->type_support->get_estimated_serialized_size( ros_message, publisher_data->type_support_impl); return publish_with_method_selection( - publisher_data, - max_data_length, - ros_message); + publisher_data, + max_data_length, + ros_message); } - - - - - - - - - - - //============================================================================== /// Publish a loaned ROS message. rmw_ret_t From a0538f0794172d4a50afe8758c2da7fd9f6ab017 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 25 Sep 2024 10:51:37 +0300 Subject: [PATCH 39/41] Codestyle --- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 2 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 61 ++++++++++++----------- 2 files changed, 33 insertions(+), 30 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 3ce2ecc3..8a71b1f3 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -171,7 +171,7 @@ size_t zenoh_shm_message_size_threshold() zenoh_shm_message_size_threshold_envar); return zenoh_shm_message_size_threshold_default; } - + // If the environment variable contains a value, handle it accordingly. if (envar_value[0] != '\0') { const auto read_value = std::strtoull(envar_value, nullptr, 10); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 17fa015b..4b18ba4e 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -128,17 +128,18 @@ create_map_and_set_sequence_num( } rmw_ret_t publish( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - z_moved_bytes_t* payload) { - + rmw_zenoh_cpp::rmw_publisher_data_t * publisher_data, + z_moved_bytes_t * payload) +{ auto free_payload = rcpputils::make_scope_exit( - [&payload]() { z_drop(payload); }); + [&payload]() {z_drop(payload);}); z_owned_bytes_t attachment; if (!create_map_and_set_sequence_num( - &attachment, - publisher_data->get_next_sequence_number(), - publisher_data->pub_gid)) { + &attachment, + publisher_data->get_next_sequence_number(), + publisher_data->pub_gid)) + { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } @@ -163,9 +164,10 @@ rmw_ret_t publish( /// Publish as RAW message. template rmw_ret_t publish_raw( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + rmw_zenoh_cpp::rmw_publisher_data_t * publisher_data, size_t max_data_length, - SerializerArgs... serializer_args) { + SerializerArgs... serializer_args) +{ // printf(">>> rmw_publish(), Will use RAW\n"); rcutils_allocator_t * allocator = @@ -191,13 +193,13 @@ rmw_ret_t publish_raw( msg_bytes, max_data_length, publisher_data, - serializer_args...); + serializer_args ...); // Return error upon unsuccessful serialization if (data_length == 0) { // serialize_into already set the error return RMW_RET_ERROR; } - + z_owned_bytes_t payload; if (z_bytes_serialize_from_buf( &payload, reinterpret_cast(msg_bytes), data_length) != Z_OK) @@ -213,12 +215,13 @@ rmw_ret_t publish_raw( /// Publish as SHM message. template rmw_ret_t publish_shm( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + rmw_zenoh_cpp::rmw_publisher_data_t * publisher_data, size_t max_data_length, - const z_loaned_shm_provider_t *provider, - SerializerArgs... serializer_args) { + const z_loaned_shm_provider_t * provider, + SerializerArgs... serializer_args) +{ // printf(">>> rmw_publish(), Will use SHM\n"); - + // Allocate SHM bufer // We use 1-byte alignment z_buf_layout_alloc_result_t alloc; @@ -243,12 +246,12 @@ rmw_ret_t publish_shm( }); // Serialize message into memory - char* msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); + char * msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); const size_t data_length = Tserializer::serialize_into( msg_bytes, max_data_length, publisher_data, - serializer_args...); + serializer_args ...); // Return error upon unsuccessful serialization if (data_length == 0) { @@ -256,7 +259,7 @@ rmw_ret_t publish_shm( return RMW_RET_ERROR; } - // construct z_owned_bytes_t from SHM buffer + // construct z_owned_bytes_t from SHM buffer z_owned_bytes_t payload; if (z_bytes_serialize_from_shm_mut(&payload, z_move(alloc.buf)) != Z_OK) { RMW_SET_ERROR_MSG("unable to serialize SHM buffer into Zenoh Payload"); @@ -271,19 +274,19 @@ rmw_ret_t publish_shm( /// Publish using raw or SHM(if applicable) buffer template rmw_ret_t publish_with_method_selection( - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, + rmw_zenoh_cpp::rmw_publisher_data_t * publisher_data, size_t max_data_length, SerializerArgs... serializer_args) { #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY if (publisher_data->context->impl->shm.has_value() && - publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) + publisher_data->context->impl->shm.value().msgsize_threshold <= max_data_length) { return publish_shm( publisher_data, max_data_length, z_loan(publisher_data->context->impl->shm.value().shm_provider), - serializer_args...); + serializer_args ...); } else #endif { @@ -988,10 +991,10 @@ rmw_return_loaned_message_from_publisher( struct RosMsgSerializer { static size_t serialize_into( - char *buffer, + char * buffer, size_t size, - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - const void *ros_message) + rmw_zenoh_cpp::rmw_publisher_data_t * publisher_data, + const void * ros_message) { // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(buffer, size); @@ -1015,10 +1018,10 @@ struct RosMsgSerializer struct RosSerializedMsgSerializer { static size_t serialize_into( - char *_buffer, + char * _buffer, size_t _size, - rmw_zenoh_cpp::rmw_publisher_data_t* publisher_data, - const rmw_serialized_message_t *serialized_message) + rmw_zenoh_cpp::rmw_publisher_data_t * publisher_data, + const rmw_serialized_message_t * serialized_message) { eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), @@ -1156,8 +1159,8 @@ rmw_publish_serialized_message( return RMW_RET_ERROR); return publish_with_method_selection( - publisher_data, - serialized_message->buffer_length, + publisher_data, + serialized_message->buffer_length, serialized_message); } From 47397323b922ee90bb52c11d474ee77c2efa42b6 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 25 Sep 2024 10:54:25 +0300 Subject: [PATCH 40/41] Update rmw_zenoh.cpp --- 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 4b18ba4e..99a5e80e 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1001,7 +1001,7 @@ struct RosMsgSerializer // Object that serializes the data rmw_zenoh_cpp::Cdr ser(fastbuffer); - + // Serialize message if (!publisher_data->type_support->serialize_ros_message( ros_message, ser.get_cdr(), publisher_data->type_support_impl)) From 728a09481d727a220e418f2e134971dcb192a235 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 25 Sep 2024 10:57:16 +0300 Subject: [PATCH 41/41] Update rmw_zenoh.cpp --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 99a5e80e..df181093 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -287,14 +287,12 @@ rmw_ret_t publish_with_method_selection( max_data_length, z_loan(publisher_data->context->impl->shm.value().shm_provider), serializer_args ...); - } else -#endif - { - return publish_raw( - publisher_data, - max_data_length, - serializer_args ...); } +#endif + return publish_raw( + publisher_data, + max_data_length, + serializer_args ...); } } // namespace