From e2fabb6e9d0ae368c295d5dba8b1b4ded8763b5f Mon Sep 17 00:00:00 2001 From: Luca Cominardi Date: Tue, 23 Jul 2024 09:48:03 +0200 Subject: [PATCH 01/77] 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 6cd6dbe877507bd4ad55e3f6afb766ffe0d7c0f4 Mon Sep 17 00:00:00 2001 From: Luca Cominardi Date: Tue, 23 Jul 2024 09:48:13 +0200 Subject: [PATCH 02/77] 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 dcefb683f5a4aff3b04ebadee2a051a62a7ba5c6 Mon Sep 17 00:00:00 2001 From: Luca Cominardi Date: Tue, 23 Jul 2024 09:48:29 +0200 Subject: [PATCH 03/77] 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 14fe58c86b6a92ccdd019102e5e0b86ca5f3cb7f Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 20 Jun 2024 14:44:40 +0800 Subject: [PATCH 04/77] 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 301a8ce3d40fd44b5c5e010d5eaa69e87ba7456a Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 25 Jun 2024 18:09:27 +0800 Subject: [PATCH 05/77] 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 00e78be6..5d5a09c0 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 8d2b6802..50445c03 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 8ef92b55051c6da32256f978bf2698cfe2085888 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 18 Jul 2024 18:08:27 +0800 Subject: [PATCH 06/77] 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 06b6fa45bfb7cd19e20a8c589f830605b40f2677 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 25 Jul 2024 17:57:30 +0800 Subject: [PATCH 07/77] 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 71e7c5bae13d2a32e7877cc60f9c7324126cd28b Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Fri, 26 Jul 2024 15:00:23 +0800 Subject: [PATCH 08/77] 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 a717270ffb6f459dfb98b83aa19d3cb6320e0e83 Mon Sep 17 00:00:00 2001 From: Mahmoud Mazouz Date: Wed, 24 Jul 2024 12:10:39 +0200 Subject: [PATCH 09/77] 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 cfa63d95615c1a1b9cc3261d892d77eb6c816c42 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 30 Jul 2024 15:03:29 +0800 Subject: [PATCH 10/77] 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 12dc820f66a40e5661a339532f81add5d3f06c68 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 15:32:00 +0800 Subject: [PATCH 11/77] 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 d331b747275838e5c1e0870a995a32b70969c6c6 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 15:34:14 +0800 Subject: [PATCH 12/77] 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 1db4bef88d781a62958e0519ff291d6893c88322 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 16:01:27 +0800 Subject: [PATCH 13/77] 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 d12f189eaa644b0822dc10cb747329f3aa908313 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 16:14:47 +0800 Subject: [PATCH 14/77] 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 909329b1db5022fb25f53bd91f4ecba9992aeea1 Mon Sep 17 00:00:00 2001 From: Mahmoud Mazouz Date: Fri, 26 Jul 2024 18:53:53 +0200 Subject: [PATCH 15/77] 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 511c6f6e6f1f74119ed6a3c97d2a353f87a5fae4 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 31 Jul 2024 18:27:29 +0800 Subject: [PATCH 16/77] 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 044d25bab826a5f1c0bc413a6ea4707ddb30662f Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Sat, 3 Aug 2024 00:17:01 +0800 Subject: [PATCH 17/77] 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 d79cf5e69d7da12f4e370474060c41eddaf5f626 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 7 Aug 2024 12:09:29 +0800 Subject: [PATCH 18/77] 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 66374004a8ab7730fee2b5180808d6d5cf511a74 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 8 Aug 2024 16:25:41 +0800 Subject: [PATCH 19/77] 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 83334c89791dc87f96c180638b066eddb5f49645 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 8 Aug 2024 16:26:35 +0800 Subject: [PATCH 20/77] 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 bf0da8a1501782a95cda232f455d83f659c14d45 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 13 Aug 2024 16:19:02 +0800 Subject: [PATCH 21/77] chore: adpat the new changes from zenoh-c and fix the bug in liveliness --- rmw_zenoh_cpp/src/rmw_init.cpp | 24 ++++++------ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 65 ++++++++++----------------------- 2 files changed, 30 insertions(+), 59 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 4154e2ef..66f67b67 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 @@ -92,6 +93,7 @@ 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) { + 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, @@ -176,6 +178,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { return ret; } + // TODO(yuyuan): SHM z_owned_string_t shm_enabled; zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled); @@ -220,6 +223,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { } } + // 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) { @@ -309,17 +313,13 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { // 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()); + 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)) { + while (z_recv(z_loan(handler), &reply) == Z_OK) { 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; @@ -329,7 +329,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { // query and the liveliness subscription. context->impl->graph_cache->parse_put(str, true); } else { - printf("[discovery] Received an error\n"); + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "[discovery] Received an error\n"); } } @@ -353,9 +353,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { 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()); + z_view_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); zc_liveliness_declare_subscriber( &context->impl->graph_subscriber, @@ -375,8 +373,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(); @@ -390,6 +386,8 @@ 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 886dd0fa..26689c5f 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -197,6 +197,7 @@ 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, @@ -418,6 +419,7 @@ rmw_publisher_t *rmw_create_publisher( 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, @@ -859,6 +861,7 @@ create_map_and_set_sequence_num(int64_t sequence_number, 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); @@ -933,41 +936,6 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, 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( - // &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); @@ -997,13 +965,13 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // 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 = &attachment; + options.attachment = z_move(attachment); z_owned_bytes_t payload; // TODO(yuyuan): SHM if (shmbuf.has_value()) { - z_bytes_serialize_from_shm_mut(&payload, &shmbuf.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 { z_bytes_serialize_from_buf( @@ -1122,7 +1090,7 @@ rmw_ret_t rmw_publish_serialized_message( // 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 = &attachment; + options.attachment = z_move(attachment); z_owned_bytes_t payload; z_bytes_serialize_from_buf(&payload, serialized_message->buffer, data_length); @@ -1479,13 +1447,13 @@ rmw_subscription_t *rmw_create_subscription( 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)) { + if (sub == nullptr || z_undeclare_subscriber(z_move(*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)) { + ze_undeclare_querying_subscriber(z_move(*querying_sub))) { RMW_SET_ERROR_MSG("failed to undeclare sub"); } } @@ -1580,7 +1548,7 @@ rmw_ret_t rmw_destroy_subscription(rmw_node_t *node, z_owned_subscriber_t *sub = std::get_if(&sub_data->sub); if (sub != nullptr) { - if (z_undeclare_subscriber(sub)) { + if (z_undeclare_subscriber(z_move(*sub))) { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; } @@ -1588,7 +1556,7 @@ rmw_ret_t rmw_destroy_subscription(rmw_node_t *node, ze_owned_querying_subscriber_t *querying_sub = std::get_if(&sub_data->sub); if (querying_sub == nullptr || - ze_undeclare_querying_subscriber(querying_sub)) { + ze_undeclare_querying_subscriber(z_move(*querying_sub))) { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; } @@ -2383,7 +2351,7 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, // rmw_client_data_t class for why we need to do this. client_data->increment_in_flight_callbacks(); - opts.attachment = &attachment; + opts.attachment = z_move(attachment); opts.target = Z_QUERY_TARGET_ALL_COMPLETE; // The default timeout for a z_get query is 10 seconds and if a response is @@ -2397,9 +2365,12 @@ 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, + + z_owned_bytes_t payload; + z_bytes_serialize_from_buf(&payload, reinterpret_cast(request_bytes), data_length); + opts.payload = z_move(payload); z_owned_closure_reply_t callback; z_closure(&callback, rmw_zenoh_cpp::client_data_handler, NULL, client_data); @@ -3045,12 +3016,14 @@ rmw_ret_t rmw_send_response(const rmw_service_t *service, z_query_reply_options_t options; z_query_reply_options_default(&options); - *options.attachment = create_map_and_set_sequence_num( + z_owned_bytes_t attachment = create_map_and_set_sequence_num( request_header->sequence_number, request_header->writer_guid); - if (!z_check(*options.attachment)) { + if (!z_check(attachment)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } + options.attachment = z_move(attachment); + z_owned_bytes_t payload; z_bytes_serialize_from_buf( &payload, reinterpret_cast(response_bytes), data_length); From dfc95ade620952af587dc55464d0697efad15301 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Sun, 18 Aug 2024 22:18:54 +0800 Subject: [PATCH 22/77] 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 cd071e57801017ea739fc8495c472c3c9b91881c Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Mon, 26 Aug 2024 15:09:31 +0800 Subject: [PATCH 23/77] fix: workaround the ZID parsing issue --- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index 26d5d1b6..41831dad 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -15,6 +15,7 @@ #include "liveliness_utils.hpp" #include +#include #include #include #include @@ -296,14 +297,11 @@ std::optional keyexpr_to_qos(const std::string & keyexpr) ///============================================================================= std::string zid_to_str(const z_id_t & id) { - std::stringstream ss; - ss << std::hex; - for (std::size_t i = 0; i < sizeof(id.id); i++) { - // By Zenoh convention a z_id_t is a little endian u128. - const std::size_t le_idx = sizeof(id.id) - 1 - i; - ss << static_cast(id.id[le_idx]); + std::ostringstream oss; + for (int i = sizeof(id.id) - 1; i >= 0; i--) { + oss << std::setw(2) << std::setfill('0') << std::hex << static_cast(id.id[i]); } - return ss.str(); + return oss.str(); } ///============================================================================= From 4f842fd47b3ea3f80c86cce2da58fc922d0fa1d8 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 22 Aug 2024 10:34:24 +0300 Subject: [PATCH 24/77] 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 af2e344dcef624e6460fca461aa1cc2357696917 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 23 Aug 2024 17:32:40 +0300 Subject: [PATCH 25/77] 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 36140fddac33317c87e9431a58b3ecf1345a0f4e Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Tue, 27 Aug 2024 14:54:29 +0200 Subject: [PATCH 26/77] chore: update to self-hosted runners --- .github/workflows/build.yaml | 5 ++++- .github/workflows/check_logging.yaml | 5 ++++- .github/workflows/style.yaml | 5 ++++- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 56714ca5..1d5e5538 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -28,7 +28,10 @@ jobs: BUILD_TYPE: binary env: ROS2_REPOS_FILE_URL: 'https://raw.githubusercontent.com/ros2/ros2/${{ matrix.ROS_DISTRO }}/ros2.repos' - runs-on: [self-hosted, ubuntu-24.04] + runs-on: + - self-hosted + - X64 + - ubuntu-ros container: image: ${{ matrix.BUILD_TYPE == 'binary' && format('ros:{0}-ros-base', matrix.ROS_DISTRO) || 'ubuntu:noble' }} steps: diff --git a/.github/workflows/check_logging.yaml b/.github/workflows/check_logging.yaml index 9e543644..453f90ab 100644 --- a/.github/workflows/check_logging.yaml +++ b/.github/workflows/check_logging.yaml @@ -4,7 +4,10 @@ name: "Check logging macros" on: [pull_request] jobs: check_logging: - runs-on: [self-hosted, ubuntu-24.04] + runs-on: + - self-hosted + - X64 + - ubuntu-ros steps: - name: Check logging macros uses: JJ/github-pr-contains-action@releases/v14.1 diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index 2a4d6531..70b74e75 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -8,7 +8,10 @@ defaults: shell: bash jobs: test: - runs-on: [self-hosted, ubuntu-24.04] + runs-on: + - self-hosted + - X64 + - ubuntu-ros strategy: fail-fast: false matrix: From 080eb1554eb87af03a0f3f727eb796db78a0ae9f Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 29 Aug 2024 14:27:48 +0800 Subject: [PATCH 27/77] fix: adapt the latest change of batching config --- .../config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 | 13 ++++++++++--- .../config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 | 13 ++++++++++--- 2 files changed, 20 insertions(+), 6 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..5ca1480e 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 @@ -183,9 +183,16 @@ data_low: 4, background: 4, }, - /// The initial exponential backoff time in nanoseconds to allow the batching to eventually progress. - /// Higher values lead to a more aggressive batching but it will introduce additional latency. - backoff: 100, + /// Perform batching of messages if they are smaller of the batch_size + batching: { + /// Perform adaptive batching of messages if they are smaller of the batch_size. + /// When the network is detected to not be fast enough to transmit every message individually, many small messages may be + /// batched together and sent all at once on the wire reducing the overall network overhead. This is typically of a high-throughput + /// scenario mainly composed of small messages. In other words, batching is activated by the network back-pressure. + enabled: true, + /// The maximum time limit (in ms) a message should be retained for batching when back-pressure happens. + time_limit: 1, + } }, // Number of threads dedicated to transmission // By default, the number of threads is calculated as follows: 1 + ((#cores - 1) / 4) 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..d788401b 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 @@ -186,9 +186,16 @@ data_low: 4, background: 4, }, - /// The initial exponential backoff time in nanoseconds to allow the batching to eventually progress. - /// Higher values lead to a more aggressive batching but it will introduce additional latency. - backoff: 100, + /// Perform batching of messages if they are smaller of the batch_size + batching: { + /// Perform adaptive batching of messages if they are smaller of the batch_size. + /// When the network is detected to not be fast enough to transmit every message individually, many small messages may be + /// batched together and sent all at once on the wire reducing the overall network overhead. This is typically of a high-throughput + /// scenario mainly composed of small messages. In other words, batching is activated by the network back-pressure. + enabled: true, + /// The maximum time limit (in ms) a message should be retained for batching when back-pressure happens. + time_limit: 1, + } }, // Number of threads dedicated to transmission // By default, the number of threads is calculated as follows: 1 + ((#cores - 1) / 4) From 83a26f2d1911bbb61c786f445b3f4477e20e969b Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 29 Aug 2024 22:00:14 +0800 Subject: [PATCH 28/77] build: deprecate the zenohc_debug and incldue the zenohc dependency in the zenoh_c_vendor --- rmw_zenoh_cpp/CMakeLists.txt | 4 ---- zenoh_c_vendor/CMakeLists.txt | 2 ++ 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 4ecfe360..a8674797 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -22,10 +22,6 @@ find_package(rosidl_typesupport_fastrtps_c REQUIRED) find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) find_package(rmw REQUIRED) find_package(zenoh_c_vendor REQUIRED) -find_package(zenohc_debug QUIET) -if(NOT zenohc_debug_FOUND) - find_package(zenohc REQUIRED) -endif() add_library(rmw_zenoh_cpp SHARED src/detail/attachment_helpers.cpp diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index f1870337..8f819c1f 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -31,4 +31,6 @@ ament_vendor(zenoh_c_vendor "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" ) +ament_export_dependencies(zenohc) + ament_package() From 31a76fde0910b07a015d42171b974b4cc53a5502 Mon Sep 17 00:00:00 2001 From: ChenYing Kuo Date: Mon, 2 Sep 2024 14:30:47 +0800 Subject: [PATCH 29/77] Use main branch for upgrading to Zenoh 1.0 Signed-off-by: ChenYing Kuo --- 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 8f819c1f..263ab7fe 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -25,7 +25,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - 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 + VCS_VERSION main CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" From b5cdd73488d9db1345a3f34146562bb06ad7c9bd Mon Sep 17 00:00:00 2001 From: "ChenYing Kuo (CY)" Date: Wed, 4 Sep 2024 17:59:58 +0800 Subject: [PATCH 30/77] Increase the delay in scouting (#16) Signed-off-by: ChenYing Kuo --- rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 | 6 +++--- rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 | 6 +++--- 2 files changed, 6 insertions(+), 6 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 5ca1480e..48acfe9d 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 @@ -29,10 +29,10 @@ }, /// Configure the scouting mechanisms and their behaviours scouting: { - /// In client mode, the period dedicated to scouting for a router before failing + /// In client mode, the period in milliseconds dedicated to scouting for a router before failing. timeout: 3000, - /// In peer mode, the period dedicated to scouting remote peers before attempting other operations - delay: 1, + /// In peer mode, the maximum period in milliseconds dedicated to scouting remote peers before attempting other operations. + delay: 500, /// The multicast scouting configuration. multicast: { /// Whether multicast scouting is enabled or not 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 d788401b..be42dc5c 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 @@ -32,10 +32,10 @@ }, /// Configure the scouting mechanisms and their behaviours scouting: { - /// In client mode, the period dedicated to scouting for a router before failing + /// In client mode, the period in milliseconds dedicated to scouting for a router before failing. timeout: 3000, - /// In peer mode, the period dedicated to scouting remote peers before attempting other operations - delay: 1, + /// In peer mode, the maximum period in milliseconds dedicated to scouting remote peers before attempting other operations. + delay: 500, /// The multicast scouting configuration. multicast: { /// Whether multicast scouting is enabled or not From aed64e850ee3b88dbf563390b27f402120e59f6e Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 19:10:50 +0800 Subject: [PATCH 31/77] fixup! Merge remote-tracking branch 'origin/rolling' into dev/1.0.0 --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 218 ++++++++++++-------------------- 1 file changed, 83 insertions(+), 135 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 6d165610..e53473bd 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include #include @@ -27,12 +27,11 @@ #include #include #include -#include #include "detail/attachment_helpers.hpp" #include "detail/cdr.hpp" -#include "detail/graph_cache.hpp" #include "detail/guard_condition.hpp" +#include "detail/graph_cache.hpp" #include "detail/identifier.hpp" #include "detail/liveliness_utils.hpp" #include "detail/logging_macros.hpp" @@ -52,14 +51,14 @@ #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 +{ //============================================================================== const rosidl_message_type_support_t * find_message_type_support( const rosidl_message_type_support_t * type_supports) @@ -227,8 +226,9 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, allocator->deallocate(node_data, allocator->state); }); - RMW_TRY_PLACEMENT_NEW(node_data, node_data, return nullptr, - rmw_zenoh_cpp::rmw_node_data_t); + 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); @@ -238,11 +238,12 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, // 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", @@ -250,9 +251,9 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, return nullptr; } - std::string keyexpr_str = node_data->entity->keyexpr(); + std::string liveliness_keyexpr = node_data->entity->liveliness_keyexpr(); z_view_keyexpr_t keyexpr; - if (z_view_keyexpr_from_str(&keyexpr, keyexpr_str.c_str())) { + if (z_view_keyexpr_from_str(&keyexpr, liveliness_keyexpr.c_str())) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to generate keyexpr from the entity string."); return nullptr; @@ -528,15 +529,6 @@ rmw_publisher_t *rmw_create_publisher( allocator->deallocate(type_hash_c_str, allocator->state); }); - 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)) { - RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); - return nullptr; - } - const z_id_t zid = z_info_zid(z_loan(node->context->impl->session)); publisher_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( @@ -561,10 +553,10 @@ rmw_publisher_t *rmw_create_publisher( rmw_publisher->topic_name); return nullptr; } - z_owned_keyexpr_t keyexpr = z_keyexpr_new( - publisher_data->entity->topic_info()->topic_keyexpr_.c_str()); - auto always_free_ros_keyexpr = rcpputils::make_scope_exit( - [&keyexpr]() { z_keyexpr_drop(z_move(keyexpr)); }); + + std::string topic_keyexpr = publisher_data->entity->topic_info()->topic_keyexpr_; + z_view_keyexpr_t pub_ke; + z_view_keyexpr_from_str(&pub_ke, topic_keyexpr.c_str()); // Create a Publication Cache if durability is transient_local. if (publisher_data->adapted_qos_profile.durability == @@ -579,18 +571,18 @@ rmw_publisher_t *rmw_create_publisher( // When such a prefix is added to the PublicationCache, it listens to queries with this extra // prefix (allowing to be queried in a unique way), but still replies with the original // publications' key expressions. - z_owned_keyexpr_t queryable_prefix = z_keyexpr_new(publisher_data->entity->zid().c_str()); - auto always_free_queryable_prefix = rcpputils::make_scope_exit( - [&queryable_prefix]() { - z_keyexpr_drop(z_move(queryable_prefix)); - }); - pub_cache_opts.queryable_prefix = z_loan(queryable_prefix); - publisher_data->pub_cache = ze_declare_publication_cache( + std::string queryable_prefix = publisher_data->entity->zid(); + z_view_keyexpr_t prefix_ke; + z_view_keyexpr_from_str(&prefix_ke, queryable_prefix.c_str()); + pub_cache_opts.queryable_prefix = z_loan(prefix_ke); + + ze_owned_publication_cache_t pub_cache; + if(ze_declare_publication_cache( + &pub_cache, z_loan(context_impl->session), - z_loan(keyexpr), + z_loan(pub_ke), &pub_cache_opts - ); - if (!publisher_data->pub_cache.has_value() || !z_check(publisher_data->pub_cache.value())) { + )) { RMW_SET_ERROR_MSG("unable to create zenoh publisher cache"); return nullptr; } @@ -616,7 +608,7 @@ rmw_publisher_t *rmw_create_publisher( // 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) { + z_loan(pub_ke), &opts) < 0) { RMW_SET_ERROR_MSG("unable to create zenoh publisher"); return nullptr; } @@ -624,28 +616,21 @@ rmw_publisher_t *rmw_create_publisher( z_undeclare_publisher(z_move(publisher_data->pub)); }); - publisher_data->token = zc_liveliness_declare_token( + std::string liveliness_keyexpr = publisher_data->entity->liveliness_keyexpr(); + z_view_keyexpr_t liveliness_ke; + z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); + if(zc_liveliness_declare_token( + &publisher_data->token, z_loan(node->context->impl->session), - z_keyexpr(publisher_data->entity->liveliness_keyexpr().c_str()), + z_loan(liveliness_ke), 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)) { + )) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the 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(); free_topic_name.cancel(); @@ -1356,31 +1341,31 @@ rmw_subscription_t *rmw_create_subscription( rmw_subscription->topic_name); return nullptr; } - z_owned_closure_sample_t callback = z_closure(rmw_zenoh_cpp::sub_data_handler, nullptr, sub_data); - z_owned_keyexpr_t keyexpr = z_keyexpr_new(sub_data->entity->topic_info()->topic_keyexpr_.c_str()); - auto always_free_ros_keyexpr = rcpputils::make_scope_exit( - [&keyexpr]() { - z_keyexpr_drop(z_move(keyexpr)); - }); - if (!z_keyexpr_check(&keyexpr)) { + z_owned_closure_sample_t callback; + z_closure(&callback, rmw_zenoh_cpp::sub_data_handler, nullptr, sub_data); + + std::string topic_keyexpr = sub_data->entity->topic_info()->topic_keyexpr_; + z_view_keyexpr_t sub_ke; + if (z_view_keyexpr_from_str(&sub_ke, topic_keyexpr.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 (sub_data->adapted_qos_profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { - ze_querying_subscriber_options_t sub_options = ze_querying_subscriber_options_default(); + ze_querying_subscriber_options_t sub_options; + ze_querying_subscriber_options_default(&sub_options); // Make the initial query to hit all the PublicationCaches, using a query_selector with // '*' in place of the queryable_prefix of each PublicationCache const std::string selector = "*/" + sub_data->entity->topic_info()->topic_keyexpr_; - sub_options.query_selector = z_keyexpr(selector.c_str()); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, selector.c_str()); + sub_options.query_selector = z_loan(ke); // Tell the PublicationCache's Queryable that the query accepts any key expression as a reply. // By default a query accepts only replies that matches its query selector. // This allows us to selectively query certain PublicationCaches when defining the // set_querying_subscriber_callback below. - sub_options.query_accept_replies = ZCU_REPLY_KEYEXPR_ANY; + sub_options.query_accept_replies = ZC_REPLY_KEYEXPR_ANY; // As this initial query is now using a "*", the query target is not COMPLETE. sub_options.query_target = Z_QUERY_TARGET_ALL; // We set consolidation to none as we need to receive transient local messages @@ -1394,11 +1379,12 @@ rmw_subscription_t *rmw_create_subscription( ze_owned_querying_subscriber_t sub; if (ze_declare_querying_subscriber(&sub, z_loan(context_impl->session), - z_loan(keyexpr), z_move(callback), + z_loan(ke), z_move(callback), &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } + // Register the querying subscriber with the graph cache to get latest // messages from publishers that were discovered after their first publication. context_impl->graph_cache->set_querying_subscriber_callback( @@ -1411,18 +1397,21 @@ rmw_subscription_t *rmw_create_subscription( const std::string selector = queryable_prefix + "/" + sub_data->entity->topic_info()->topic_keyexpr_; + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, selector.c_str()); RMW_ZENOH_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", "QueryingSubscriberCallback triggered over %s.", selector.c_str() ); - z_get_options_t opts = z_get_options_default(); + z_get_options_t opts; + z_get_options_default(&opts); opts.timeout_ms = std::numeric_limits::max(); opts.consolidation = z_query_consolidation_latest(); - opts.accept_replies = ZCU_REPLY_KEYEXPR_ANY; + opts.accept_replies = ZC_REPLY_KEYEXPR_ANY; ze_querying_subscriber_get( z_loan(std::get(sub_data->sub)), - z_keyexpr(selector.c_str()), + z_loan(ke), &opts ); } @@ -1436,7 +1425,7 @@ rmw_subscription_t *rmw_create_subscription( } z_owned_subscriber_t sub; - if(z_declare_subscriber(&sub, z_loan(context_impl->session), z_loan(keyexpr), z_move(callback), &sub_options)) { + if(z_declare_subscriber(&sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } @@ -1459,18 +1448,15 @@ rmw_subscription_t *rmw_create_subscription( }); // Publish to the graph that a new subscription is in town. - sub_data->token = zc_liveliness_declare_token( + std::string liveliness_keyexpr = sub_data->entity->liveliness_keyexpr(); + z_view_keyexpr_t liveliness_ke; + z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); + if (zc_liveliness_declare_token( + &sub_data->token, z_loan(context_impl->session), - z_keyexpr(sub_data->entity->liveliness_keyexpr().c_str()), + z_loan(liveliness_ke), 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."); @@ -2153,40 +2139,25 @@ rmw_client_t *rmw_create_client( return nullptr; } - client_data->keyexpr = z_keyexpr_new(client_data->entity->topic_info()->topic_keyexpr_.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)) { + if (z_keyexpr_from_str(&client_data->keyexpr, client_data->entity->topic_info()->topic_keyexpr_.c_str())) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } - client_data->token = zc_liveliness_declare_token( + std::string liveliness_keyexpr = client_data->entity->liveliness_keyexpr(); + z_view_keyexpr_t liveliness_ke; + z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); + if(zc_liveliness_declare_token( + &client_data->token, z_loan(node->context->impl->session), - z_keyexpr(client_data->entity->liveliness_keyexpr().c_str()), + z_loan(liveliness_ke), 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; } - // 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); - z_drop(z_move(keyexpr)); rmw_client->data = client_data; @@ -2198,7 +2169,6 @@ rmw_client_t *rmw_create_client( free_response_type_support.cancel(); destruct_client_data.cancel(); free_service_name.cancel(); - free_ros_keyexpr.cancel(); return rmw_client; } @@ -2696,22 +2666,10 @@ rmw_service_t *rmw_create_service( rmw_service->service_name); return nullptr; } - service_data->keyexpr = z_keyexpr_new(service_data->entity->topic_info()->topic_keyexpr_.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))) { + if(z_keyexpr_from_str(&service_data->keyexpr, service_data->entity->topic_info()->topic_keyexpr_.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)); - } - }); z_owned_closure_query_t callback; z_closure(&callback, rmw_zenoh_cpp::service_data_handler, nullptr, @@ -2720,36 +2678,29 @@ rmw_service_t *rmw_create_service( z_queryable_options_t qable_options; z_queryable_options_default(&qable_options); qable_options.complete = true; - z_result_t z_ret = z_declare_queryable( + if(z_declare_queryable( &service_data->qable, z_loan(context_impl->session), - z_loan(service_data->keyexpr), z_move(callback), &qable_options); - - if (z_ret) { + z_loan(service_data->keyexpr), z_move(callback), &qable_options)) { 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->token = zc_liveliness_declare_token( + std::string liveliness_keyexpr = service_data->entity->liveliness_keyexpr(); + z_view_keyexpr_t liveliness_ke; + z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); + if (zc_liveliness_declare_token( + &service_data->token, z_loan(node->context->impl->session), - z_keyexpr(service_data->entity->liveliness_keyexpr().c_str()), + z_loan(liveliness_ke), 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)) { + )) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the service."); return nullptr; } - z_drop(z_move(keyexpr)); - rmw_service->data = service_data; free_rmw_service.cancel(); @@ -2760,10 +2711,7 @@ rmw_service_t *rmw_create_service( destruct_response_type_support.cancel(); free_request_type_support.cancel(); free_response_type_support.cancel(); - free_ros_keyexpr.cancel(); undeclare_z_queryable.cancel(); - free_token.cancel(); - free_keyexpr.cancel(); return rmw_service; } From dd0e541db9e30c2df218794c246933843a252025 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 21:14:46 +0800 Subject: [PATCH 32/77] chore: ament_uncrustify and ament_cpplint --- .../src/detail/attachment_helpers.cpp | 38 +- .../src/detail/attachment_helpers.hpp | 33 +- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 1 + rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 2 +- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 162 ++- .../src/detail/zenoh_router_check.cpp | 2 +- .../src/detail/zenoh_router_check.hpp | 2 +- rmw_zenoh_cpp/src/rmw_event.cpp | 3 - rmw_zenoh_cpp/src/rmw_init.cpp | 93 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 1186 +++++++++-------- 10 files changed, 838 insertions(+), 684 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 52e55dd9..c0e262f1 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -12,13 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include #include -#include #include "rmw/types.h" @@ -26,10 +24,12 @@ #include "attachment_helpers.hpp" -namespace rmw_zenoh_cpp { +namespace rmw_zenoh_cpp +{ -bool create_attachment_iter(z_owned_bytes_t *kv_pair, void *context) { - attachement_context_t *ctx = (attachement_context_t *)context; +bool create_attachment_iter(z_owned_bytes_t *kv_pair, void *context) +{ + attachement_context_t *ctx = reinterpret_cast(context); z_owned_bytes_t k, v; if (ctx->idx == 0) { @@ -51,14 +51,17 @@ bool create_attachment_iter(z_owned_bytes_t *kv_pair, void *context) { return true; } -z_result_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_from_iter(attachment, create_attachment_iter, - (void *)&context); + reinterpret_cast(&context)); } -bool get_attachment(const z_loaned_bytes_t *const attachment, - const std::string &key, z_owned_bytes_t *val) { +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; } @@ -72,7 +75,7 @@ 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); - const char* key_string_ptr = z_string_data(z_loan(key_string)); + 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; @@ -98,9 +101,10 @@ bool get_attachment(const z_loaned_bytes_t *const attachment, return true; } -bool get_gid_from_attachment(const z_loaned_bytes_t *const attachment, - uint8_t gid[RMW_GID_STORAGE_SIZE]) { - +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; } @@ -124,8 +128,10 @@ bool get_gid_from_attachment(const z_loaned_bytes_t *const attachment, return true; } -int64_t get_int64_from_attachment(const z_loaned_bytes_t *const attachment, - const std::string &name) { +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; @@ -154,4 +160,4 @@ int64_t get_int64_from_attachment(const z_loaned_bytes_t *const attachment, 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 7e6ee737..aa44de43 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -21,16 +21,19 @@ #include "rmw/types.h" -namespace rmw_zenoh_cpp { +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]) { + 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); @@ -44,17 +47,21 @@ class attachement_context_t final { int length = 3; int idx = 0; - attachement_context_t(const attachement_data_t *_data) : data(_data) {} + 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_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]); +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 +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_ +#endif // DETAIL__ATTACHMENT_HELPERS_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index c6fb5f01..e38b3262 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include "event.hpp" #include "liveliness_utils.hpp" diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index d451182e..16bda1ee 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -373,7 +373,7 @@ std::string zid_to_str(const z_id_t & id) { std::ostringstream oss; for (int i = sizeof(id.id) - 1; i >= 0; i--) { - oss << std::setw(2) << std::setfill('0') << std::hex << static_cast(id.id[i]); + oss << std::setw(2) << std::setfill('0') << std::hex << static_cast(id.id[i]); } return oss.str(); } diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 780d644c..53ffe2c4 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include @@ -22,21 +21,19 @@ #include #include #include -#include #include "logging_macros.hpp" -#include "rcpputils/scope_exit.hpp" - -#include "rmw/error_handling.h" #include "rmw/impl/cpp/macros.hpp" #include "attachment_helpers.hpp" #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; @@ -47,21 +44,25 @@ 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_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) { +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); } ///============================================================================= @@ -71,14 +72,16 @@ saved_msg_data::~saved_msg_data() } ///============================================================================= -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; @@ -90,7 +93,8 @@ 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); @@ -100,7 +104,8 @@ 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; @@ -108,7 +113,8 @@ 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()) { @@ -118,7 +124,7 @@ std::unique_ptr rmw_subscription_data_t::pop_next_message() { } std::unique_ptr msg_data = - std::move(message_queue_.front()); + std::move(message_queue_.front()); message_queue_.pop_front(); return msg_data; @@ -126,11 +132,13 @@ 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", @@ -154,7 +162,7 @@ void rmw_subscription_data_t::add_new_message( 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); + 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; @@ -178,7 +186,8 @@ void rmw_subscription_data_t::add_new_message( ///============================================================================= 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; @@ -189,7 +198,8 @@ 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; @@ -197,7 +207,8 @@ 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; @@ -210,7 +221,8 @@ 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); @@ -220,10 +232,12 @@ 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); @@ -244,14 +258,16 @@ void rmw_service_data_t::add_new_query(std::unique_ptr query) { } ///============================================================================= -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); + sequence_to_query_map_.find(hash); if (it == sequence_to_query_map_.end()) { SequenceToQuery stq; @@ -275,20 +291,21 @@ bool rmw_service_data_t::add_to_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) { +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); + 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); + it->second.find(request_id.sequence_number); if (query_it == it->second.end()) { return nullptr; @@ -305,7 +322,8 @@ rmw_service_data_t::take_from_query_map(const rmw_request_id_t &request_id) { } ///============================================================================= -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); @@ -315,10 +333,12 @@ 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); @@ -339,7 +359,8 @@ void rmw_client_data_t::add_new_reply(std::unique_ptr reply) { ///============================================================================= 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; @@ -350,7 +371,8 @@ 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; @@ -358,7 +380,8 @@ 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()) { @@ -374,7 +397,8 @@ 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() { +void rmw_client_data_t::increment_in_flight_callbacks() +{ std::lock_guard lock(in_flight_mutex_); num_in_flight_++; } @@ -382,7 +406,8 @@ 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. -bool rmw_client_data_t::shutdown_and_query_in_flight() { +bool rmw_client_data_t::shutdown_and_query_in_flight() +{ std::lock_guard lock(in_flight_mutex_); is_shutdown_ = true; @@ -393,19 +418,22 @@ 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) { + 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); @@ -430,7 +458,7 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) { } int64_t sequence_number = - get_int64_from_attachment(attachment, "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. @@ -441,7 +469,7 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) { } int64_t source_timestamp = - get_int64_from_attachment(attachment, "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. @@ -464,20 +492,23 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) { } ///============================================================================= -ZenohQuery::ZenohQuery(const z_loaned_query_t *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); @@ -495,26 +526,29 @@ 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 { +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", @@ -528,8 +562,7 @@ 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)); @@ -545,14 +578,15 @@ void client_data_handler(const z_loaned_reply_t *reply, void *data) { 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(keystr)), static_cast(z_string_len(z_loan(err_str))), z_string_data(z_loan(err_str))); z_drop(z_move(err_str)); return; } } -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", @@ -576,4 +610,4 @@ void client_data_drop(void *data) { } } -} // namespace rmw_zenoh_cpp +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp index 7d7f5cd9..74081fb9 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp @@ -24,7 +24,7 @@ namespace rmw_zenoh_cpp { ///============================================================================= -rmw_ret_t zenoh_router_check(const z_loaned_session_t* session) +rmw_ret_t zenoh_router_check(const z_loaned_session_t * session) { // Initialize context for callback int context = 0; diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp index b6a56b05..1db74a4c 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(const z_loaned_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 d9781d9d..deec2e4f 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -14,19 +14,16 @@ #include "rmw/error_handling.h" #include "rmw/event.h" -#include "rmw/events_statuses/events_statuses.h" #include "rmw/types.h" #include "detail/event.hpp" #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 7526b7a7..2a6f6981 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -12,14 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include #include #include #include -#include + #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" @@ -50,7 +48,6 @@ namespace void graph_sub_data_handler(const z_loaned_sample_t * sample, void * data) { - static_cast(data); z_view_string_t keystr; @@ -69,18 +66,18 @@ graph_sub_data_handler(const z_loaned_sample_t * sample, void * data) 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; + 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_trigger_guard_condition(context_impl->graph_guard_condition); if (RMW_RET_OK != rmw_ret) { RMW_ZENOH_LOG_WARN_NAMED( "rmw_zenoh_cpp", @@ -92,8 +89,8 @@ 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) { - +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, @@ -110,14 +107,14 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { } 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. context->actual_domain_id = - RMW_DEFAULT_DOMAIN_ID != options->domain_id ? options->domain_id : 0u; + RMW_DEFAULT_DOMAIN_ID != options->domain_id ? options->domain_id : 0u; const rcutils_allocator_t *allocator = &options->allocator; @@ -126,13 +123,13 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { 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); + 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_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(context->impl->~rmw_context_impl_t(), rmw_context_impl_t *); }); @@ -142,11 +139,11 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { 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( + 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. @@ -155,7 +152,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { "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); + allocator->deallocate(context->impl->enclave, allocator->state); }); // Initialize context's implementation @@ -173,7 +170,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { z_owned_config_t config; if ((ret = rmw_zenoh_cpp::get_z_config( rmw_zenoh_cpp::ConfigurableEntity::Session, &config)) != - RMW_RET_OK) { + RMW_RET_OK) + { RMW_SET_ERROR_MSG("Error configuring Zenoh session."); return ret; } @@ -182,7 +180,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { // 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( + auto free_shm_ = rcpputils::make_scope_exit( [&shm_enabled]() { z_drop(z_move(shm_enabled)); }); @@ -194,7 +192,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { } 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)); @@ -202,15 +200,17 @@ rmw_ret_t 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()) { + connection_attempts < configured_connection_attempts.value()) + { if ((ret = rmw_zenoh_cpp::zenoh_router_check( - z_loan(context->impl->session))) != RMW_RET_OK) { + z_loan(context->impl->session))) != RMW_RET_OK) + { ++connection_attempts; } std::this_thread::sleep_for(std::chrono::seconds(1)); @@ -251,19 +251,19 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { // Initialize the guard condition. context->impl->graph_guard_condition = - static_cast(allocator->zero_allocate( + 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]() { + 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; + rmw_zenoh_cpp::rmw_zenoh_identifier; context->impl->graph_guard_condition->data = allocator->zero_allocate( 1, sizeof(rmw_zenoh_cpp::GuardCondition), allocator->state); @@ -271,7 +271,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { "failed to allocate graph guard condition data", return RMW_RET_BAD_ALLOC); auto free_guard_condition_data = - rcpputils::make_scope_exit([context, allocator]() { + rcpputils::make_scope_exit([context, allocator]() { allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); }); @@ -281,15 +281,15 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { 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(), + 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); + rmw_zenoh_cpp::liveliness::subscription_token(context->actual_domain_id); // Query router/liveliness participants to get graph information before this // session was started. @@ -321,7 +321,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { while (z_recv(z_loan(handler), &reply) == Z_OK) { if (z_reply_is_ok(z_loan(reply))) { - const z_loaned_sample_t* sample = z_reply_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))); @@ -358,7 +358,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) { if(zc_liveliness_declare_subscriber( &context->impl->graph_subscriber, z_loan(context->impl->session), z_loan(keyexpr), - z_move(callback), &sub_options) != Z_OK) { + z_move(callback), &sub_options) != Z_OK) + { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return RMW_RET_ERROR; } @@ -380,7 +381,8 @@ rmw_ret_t 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); @@ -405,7 +407,8 @@ rmw_ret_t 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); @@ -420,8 +423,8 @@ rmw_ret_t rmw_context_fini(rmw_context_t *context) { const rcutils_allocator_t *allocator = &context->options.allocator; RMW_TRY_DESTRUCTOR(static_cast( - context->impl->graph_guard_condition->data) - ->~GuardCondition(), + context->impl->graph_guard_condition->data) + ->~GuardCondition(), rmw_zenoh_cpp::GuardCondition, ); allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); @@ -442,4 +445,4 @@ rmw_ret_t 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 e53473bd..c89da184 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include @@ -43,11 +42,8 @@ #include "rcpputils/scope_exit.hpp" -#include "rcutils/env.h" #include "rcutils/strdup.h" -#include "rcutils/types.h" -#include "rmw/allocators.h" #include "rmw/dynamic_message_type_support.h" #include "rmw/error_handling.h" #include "rmw/features.h" @@ -88,9 +84,10 @@ 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) { +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, + 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(); @@ -113,7 +110,7 @@ find_service_type_support(const rosidl_service_type_support_t *type_supports) { return type_support; } -} // namespace +} // namespace extern "C" { @@ -122,38 +119,41 @@ 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, @@ -174,7 +174,7 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, } if (RMW_NODE_NAME_VALID != validation_result) { const char *reason = - rmw_node_name_validation_result_string(validation_result); + rmw_node_name_validation_result_string(validation_result); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid node name: %s", reason); return nullptr; } @@ -186,7 +186,7 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, } if (RMW_NAMESPACE_VALID != validation_result) { const char *reason = - rmw_namespace_validation_result_string(validation_result); + rmw_namespace_validation_result_string(validation_result); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid node namespace: %s", reason); return nullptr; } @@ -194,17 +194,17 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, rcutils_allocator_t *allocator = &context->options.allocator; rmw_node_t *node = static_cast( - allocator->zero_allocate(1, sizeof(rmw_node_t), allocator->state)); + 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); + allocator->deallocate(const_cast(node->name), allocator->state); }); node->namespace_ = rcutils_strdup(namespace_, *allocator); @@ -212,25 +212,25 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, "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->deallocate(const_cast(node->namespace_), allocator->state); }); // Put metadata into node->data. auto node_data = - static_cast(allocator->allocate( + 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); + 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_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(node_data->~rmw_node_data_t(), rmw_zenoh_cpp::rmw_node_data_t); }); @@ -261,14 +261,15 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, if (zc_liveliness_declare_token(&node_data->token, z_loan(context->impl->session), - z_loan(keyexpr), NULL)) { + z_loan(keyexpr), NULL)) + { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Failed to declare liveness token."); return nullptr; } 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));}); node->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; node->context = context; @@ -286,7 +287,8 @@ rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, //============================================================================== /// Finalize a given node handle, reclaim the resources, and deallocate the node /// handle. -rmw_ret_t rmw_destroy_node(rmw_node_t *node) { +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); @@ -300,7 +302,7 @@ rmw_ret_t rmw_destroy_node(rmw_node_t *node) { // 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); + 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, ); @@ -317,7 +319,8 @@ rmw_ret_t 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, @@ -332,9 +335,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) { + 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); @@ -345,34 +349,36 @@ rmw_ret_t 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, @@ -387,31 +393,32 @@ rmw_publisher_t *rmw_create_publisher( 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_validate_full_topic_name(topic_name, &validation_result, nullptr); if (RMW_RET_OK != ret) { return nullptr; } if (RMW_TOPIC_VALID != validation_result) { const char *reason = - rmw_full_topic_name_validation_result_string(validation_result); + rmw_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_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); + 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); + find_message_type_support(type_supports); if (type_support == nullptr) { // error was already set by find_message_type_support return nullptr; @@ -424,7 +431,7 @@ rmw_publisher_t *rmw_create_publisher( 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); + 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, @@ -434,30 +441,30 @@ rmw_publisher_t *rmw_create_publisher( // Create the publisher. auto rmw_publisher = static_cast( - allocator->zero_allocate(1, sizeof(rmw_publisher_t), allocator->state)); + 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]() { + rcpputils::make_scope_exit([rmw_publisher, allocator]() { allocator->deallocate(rmw_publisher, allocator->state); }); auto publisher_data = - static_cast(allocator->allocate( + 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]() { + 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( + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( publisher_data->~rmw_publisher_data_t(), rmw_zenoh_cpp::rmw_publisher_data_t); }); @@ -477,15 +484,15 @@ rmw_publisher_t *rmw_create_publisher( 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); + static_cast(type_support->data); publisher_data->type_support = - static_cast(allocator->allocate( + 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]() { + rcpputils::make_scope_exit([publisher_data, allocator]() { allocator->deallocate(publisher_data->type_support, allocator->state); }); @@ -493,7 +500,7 @@ rmw_publisher_t *rmw_create_publisher( publisher_data->type_support, return nullptr, rmw_zenoh_cpp::MessageTypeSupport, callbacks); auto destruct_msg_type_support = - rcpputils::make_scope_exit([publisher_data]() { + rcpputils::make_scope_exit([publisher_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( publisher_data->type_support->~MessageTypeSupport(), rmw_zenoh_cpp::MessageTypeSupport); @@ -502,7 +509,7 @@ rmw_publisher_t *rmw_create_publisher( publisher_data->context = node->context; rmw_publisher->data = publisher_data; rmw_publisher->implementation_identifier = - rmw_zenoh_cpp::rmw_zenoh_identifier; + rmw_zenoh_cpp::rmw_zenoh_identifier; rmw_publisher->options = *publisher_options; rmw_publisher->can_loan_messages = false; @@ -510,7 +517,7 @@ rmw_publisher_t *rmw_create_publisher( 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]() { + rcpputils::make_scope_exit([rmw_publisher, allocator]() { allocator->deallocate(const_cast(rmw_publisher->topic_name), allocator->state); }); @@ -525,7 +532,7 @@ rmw_publisher_t *rmw_create_publisher( return nullptr; } auto free_type_hash_c_str = - rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { + rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { allocator->deallocate(type_hash_c_str, allocator->state); }); @@ -560,7 +567,8 @@ rmw_publisher_t *rmw_create_publisher( // Create a Publication Cache if durability is transient_local. if (publisher_data->adapted_qos_profile.durability == - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { + 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; @@ -582,14 +590,15 @@ rmw_publisher_t *rmw_create_publisher( z_loan(context_impl->session), z_loan(pub_ke), &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]() { + rcpputils::make_scope_exit([publisher_data]() { if (publisher_data && publisher_data->pub_cache.has_value()) { z_drop(z_move(publisher_data->pub_cache.value())); } @@ -600,20 +609,22 @@ rmw_publisher_t *rmw_create_publisher( 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) { + 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? if (z_declare_publisher(&publisher_data->pub, z_loan(context_impl->session), - z_loan(pub_ke), &opts) < 0) { + z_loan(pub_ke), &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)); + z_undeclare_publisher(z_move(publisher_data->pub)); }); std::string liveliness_keyexpr = publisher_data->entity->liveliness_keyexpr(); @@ -624,7 +635,8 @@ rmw_publisher_t *rmw_create_publisher( z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL - )) { + )) + { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the publisher."); @@ -646,7 +658,8 @@ rmw_publisher_t *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) { +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); @@ -663,7 +676,7 @@ rmw_ret_t rmw_destroy_publisher(rmw_node_t *node, rmw_publisher_t *publisher) { rcutils_allocator_t *allocator = &node->context->options.allocator; auto publisher_data = - static_cast(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()) { @@ -689,9 +702,10 @@ rmw_ret_t rmw_destroy_publisher(rmw_node_t *node, rmw_publisher_t *publisher) { //============================================================================== 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) { + 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); @@ -701,10 +715,11 @@ rmw_ret_t 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) { + 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); @@ -715,8 +730,9 @@ rmw_ret_t 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) { + 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); @@ -726,9 +742,11 @@ rmw_ret_t 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); @@ -738,64 +756,29 @@ rmw_borrow_loaned_message(const rmw_publisher_t *publisher, //============================================================================== /// 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 { +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]) { - - // z_owned_slice_map_t map; - // z_slice_map_new(&map); - - // auto free_attachment_map = - // rcpputils::make_scope_exit([&map]() { z_drop(z_move(map)); }); - - // z_view_slice_t key, val; - - // // 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; - // } - - // // 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)); - +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(); - // 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); + gid); if (data.serialize_to_zbytes(out_bytes)) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Failed to serialize the attachment"); @@ -804,14 +787,14 @@ create_map_and_set_sequence_num(z_owned_bytes_t* out_bytes, return true; } -} // 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) { - - +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); @@ -823,16 +806,16 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, return RMW_RET_INVALID_ARGUMENT); auto publisher_data = - static_cast(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); + &(publisher_data->context->options.allocator); // Serialize data. size_t max_data_length = - publisher_data->type_support->get_estimated_serialized_size( + publisher_data->type_support->get_estimated_serialized_size( ros_message, publisher_data->type_support_impl); // To store serialized message byte array. @@ -841,13 +824,13 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // TODO(yuyuan): SHM std::optional shmbuf = std::nullopt; auto always_free_shmbuf = rcpputils::make_scope_exit([&shmbuf]() { - if (shmbuf.has_value()) { + if (shmbuf.has_value()) { // TODO(yuyuan): do we need to drop z_owned_shm_mut_t? - z_drop(z_move(shmbuf.value())); - } + z_drop(z_move(shmbuf.value())); + } }); auto free_msg_bytes = - rcpputils::make_scope_exit([&msg_bytes, allocator, &shmbuf]() { + rcpputils::make_scope_exit([&msg_bytes, allocator, &shmbuf]() { if (msg_bytes && !shmbuf.has_value()) { allocator->deallocate(msg_bytes, allocator->state); } @@ -869,7 +852,7 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, 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))); + 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."); @@ -881,7 +864,7 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // Get memory from the allocator. msg_bytes = static_cast( - allocator->allocate(max_data_length, allocator->state)); + 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); } @@ -892,7 +875,8 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, // 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; } @@ -907,7 +891,7 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, return RMW_RET_ERROR; } auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() { z_drop(z_move(attachment)); }); + [&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 @@ -936,9 +920,11 @@ rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message, //============================================================================== /// 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); @@ -948,8 +934,10 @@ rmw_ret_t rmw_publish_loaned_message(const rmw_publisher_t *publisher, //============================================================================== /// 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, @@ -959,11 +947,11 @@ rmw_publisher_count_matched_subscriptions(const rmw_publisher_t *publisher, RMW_CHECK_ARGUMENT_FOR_NULL(subscription_count, RMW_RET_INVALID_ARGUMENT); rmw_zenoh_cpp::rmw_publisher_data_t *pub_data = - static_cast(publisher->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); + 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( @@ -972,8 +960,10 @@ rmw_publisher_count_matched_subscriptions(const rmw_publisher_t *publisher, //============================================================================== /// 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, @@ -982,7 +972,7 @@ rmw_ret_t rmw_publisher_get_actual_qos(const rmw_publisher_t *publisher, RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); rmw_zenoh_cpp::rmw_publisher_data_t *pub_data = - static_cast(publisher->data); + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); *qos = pub_data->adapted_qos_profile; @@ -992,9 +982,10 @@ rmw_ret_t rmw_publisher_get_actual_qos(const rmw_publisher_t *publisher, //============================================================================== /// 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) { + 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); @@ -1007,13 +998,13 @@ rmw_ret_t rmw_publish_serialized_message( return RMW_RET_INVALID_ARGUMENT); auto publisher_data = - static_cast(publisher->data); + static_cast(publisher->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG( publisher_data, "publisher data pointer is null", return RMW_RET_ERROR); eprosima::fastcdr::FastBuffer buffer( - reinterpret_cast(serialized_message->buffer), - serialized_message->buffer_length); + 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"); @@ -1028,7 +1019,7 @@ rmw_ret_t rmw_publish_serialized_message( return RMW_RET_ERROR; } auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() { z_drop(z_move(attachment)); }); + [&attachment]() {z_drop(z_move(attachment));}); const size_t data_length = ser.get_serialized_data_length(); @@ -1053,8 +1044,9 @@ rmw_ret_t 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) { + 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); @@ -1064,13 +1056,14 @@ rmw_ret_t 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_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); + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); return RMW_RET_OK; @@ -1079,8 +1072,10 @@ rmw_ret_t 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) { +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); @@ -1095,30 +1090,33 @@ rmw_ret_t rmw_publisher_wait_for_all_acked(const rmw_publisher_t *publisher, //============================================================================== /// 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) { +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); + 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); + 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) { + 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); @@ -1129,35 +1127,38 @@ rmw_ret_t rmw_serialize(const void *ros_message, //============================================================================== /// 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) { +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); + 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); + 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); + 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) { + 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); @@ -1168,17 +1169,19 @@ rmw_ret_t 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, @@ -1193,7 +1196,7 @@ rmw_subscription_t *rmw_create_subscription( RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); const rosidl_message_type_support_t *type_support = - find_message_type_support(type_supports); + find_message_type_support(type_supports); if (type_support == nullptr) { // error was already set by find_message_type_support return nullptr; @@ -1213,7 +1216,7 @@ rmw_subscription_t *rmw_create_subscription( 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); + 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, @@ -1223,30 +1226,30 @@ rmw_subscription_t *rmw_create_subscription( // Create the rmw_subscription. rmw_subscription_t *rmw_subscription = - static_cast(allocator->zero_allocate( + 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]() { + rcpputils::make_scope_exit([rmw_subscription, allocator]() { allocator->deallocate(rmw_subscription, allocator->state); }); auto sub_data = - static_cast(allocator->allocate( + 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); + 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_DESTRUCTOR_FROM_WITHIN_FAILURE( sub_data->~rmw_subscription_data_t(), rmw_zenoh_cpp::rmw_subscription_data_t); }); @@ -1265,22 +1268,22 @@ rmw_subscription_t *rmw_create_subscription( 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); + static_cast(type_support->data); sub_data->type_support = - static_cast(allocator->allocate( + 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); + 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_DESTRUCTOR_FROM_WITHIN_FAILURE( sub_data->type_support->~MessageTypeSupport(), rmw_zenoh_cpp::MessageTypeSupport); }); @@ -1288,13 +1291,13 @@ rmw_subscription_t *rmw_create_subscription( sub_data->context = node->context; rmw_subscription->implementation_identifier = - rmw_zenoh_cpp::rmw_zenoh_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]() { + rcpputils::make_scope_exit([rmw_subscription, allocator]() { allocator->deallocate(const_cast(rmw_subscription->topic_name), allocator->state); }); @@ -1313,7 +1316,7 @@ rmw_subscription_t *rmw_create_subscription( return nullptr; } auto free_type_hash_c_str = - rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { + rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { allocator->deallocate(type_hash_c_str, allocator->state); }); @@ -1373,14 +1376,16 @@ rmw_subscription_t *rmw_create_subscription( // by various publishers. sub_options.query_consolidation = z_query_consolidation_none(); if (sub_data->adapted_qos_profile.reliability == - RMW_QOS_POLICY_RELIABILITY_RELIABLE) { + RMW_QOS_POLICY_RELIABILITY_RELIABLE) + { sub_options.reliability = Z_RELIABILITY_RELIABLE; } ze_owned_querying_subscriber_t sub; if (ze_declare_querying_subscriber(&sub, z_loan(context_impl->session), z_loan(ke), z_move(callback), - &sub_options)) { + &sub_options)) + { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } @@ -1425,7 +1430,9 @@ rmw_subscription_t *rmw_create_subscription( } z_owned_subscriber_t sub; - if(z_declare_subscriber(&sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), &sub_options)) { + if(z_declare_subscriber(&sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), + &sub_options)) + { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } @@ -1433,18 +1440,19 @@ rmw_subscription_t *rmw_create_subscription( } auto undeclare_z_sub = rcpputils::make_scope_exit([sub_data]() { - z_owned_subscriber_t *sub = + z_owned_subscriber_t *sub = std::get_if(&sub_data->sub); - if (sub == nullptr || z_undeclare_subscriber(z_move(*sub))) { - RMW_SET_ERROR_MSG("failed to undeclare sub"); - } else { - ze_owned_querying_subscriber_t *querying_sub = + if (sub == nullptr || z_undeclare_subscriber(z_move(*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(z_move(*querying_sub))) { - RMW_SET_ERROR_MSG("failed to undeclare sub"); - } - } + if (querying_sub == nullptr || + ze_undeclare_querying_subscriber(z_move(*querying_sub))) + { + RMW_SET_ERROR_MSG("failed to undeclare sub"); + } + } }); // Publish to the graph that a new subscription is in town. @@ -1456,7 +1464,8 @@ rmw_subscription_t *rmw_create_subscription( z_loan(context_impl->session), z_loan(liveliness_ke), NULL - )) { + )) + { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the subscription."); @@ -1479,8 +1488,10 @@ rmw_subscription_t *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) { +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, @@ -1496,7 +1507,7 @@ rmw_ret_t rmw_destroy_subscription(rmw_node_t *node, rcutils_allocator_t *allocator = &node->context->options.allocator; auto sub_data = - static_cast(subscription->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)); @@ -1506,7 +1517,7 @@ rmw_ret_t rmw_destroy_subscription(rmw_node_t *node, allocator->deallocate(sub_data->type_support, allocator->state); z_owned_subscriber_t *sub = - std::get_if(&sub_data->sub); + std::get_if(&sub_data->sub); if (sub != nullptr) { if (z_undeclare_subscriber(z_move(*sub))) { RMW_SET_ERROR_MSG("failed to undeclare sub"); @@ -1514,9 +1525,10 @@ rmw_ret_t rmw_destroy_subscription(rmw_node_t *node, } } else { ze_owned_querying_subscriber_t *querying_sub = - std::get_if(&sub_data->sub); + std::get_if(&sub_data->sub); if (querying_sub == nullptr || - ze_undeclare_querying_subscriber(z_move(*querying_sub))) { + ze_undeclare_querying_subscriber(z_move(*querying_sub))) + { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; } @@ -1536,7 +1548,8 @@ rmw_ret_t rmw_destroy_subscription(rmw_node_t *node, //============================================================================== /// 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) { + 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, @@ -1546,11 +1559,11 @@ rmw_ret_t rmw_subscription_count_matched_publishers( RMW_CHECK_ARGUMENT_FOR_NULL(publisher_count, RMW_RET_INVALID_ARGUMENT); rmw_zenoh_cpp::rmw_subscription_data_t *sub_data = - static_cast(subscription->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); + 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( @@ -1560,8 +1573,10 @@ rmw_ret_t rmw_subscription_count_matched_publishers( //============================================================================== /// 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, @@ -1570,7 +1585,7 @@ rmw_subscription_get_actual_qos(const rmw_subscription_t *subscription, RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); auto sub_data = - static_cast(subscription->data); + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); *qos = sub_data->adapted_qos_profile; @@ -1580,8 +1595,9 @@ rmw_subscription_get_actual_qos(const rmw_subscription_t *subscription, //============================================================================== /// 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_subscription_t *subscription, + const rmw_subscription_content_filter_options_t *options) +{ static_cast(subscription); static_cast(options); return RMW_RET_UNSUPPORTED; @@ -1590,22 +1606,26 @@ rmw_ret_t 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) { + 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(); + sub_data->pop_next_message(); if (msg_data == nullptr) { // There are no more messages to take. return RMW_RET_OK; @@ -1616,12 +1636,13 @@ rmw_ret_t __rmw_take_one(rmw_zenoh_cpp::rmw_subscription_data_t *sub_data, // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(const_cast(payload)), payload_len); + reinterpret_cast(const_cast(payload)), payload_len); // 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; } @@ -1633,7 +1654,7 @@ rmw_ret_t __rmw_take_one(rmw_zenoh_cpp::rmw_subscription_data_t *sub_data, // 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; + 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; @@ -1643,12 +1664,14 @@ rmw_ret_t __rmw_take_one(rmw_zenoh_cpp::rmw_subscription_data_t *sub_data, 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); @@ -1664,7 +1687,7 @@ rmw_ret_t rmw_take(const rmw_subscription_t *subscription, void *ros_message, *taken = false; auto sub_data = - static_cast(subscription->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); @@ -1672,10 +1695,12 @@ rmw_ret_t rmw_take(const rmw_subscription_t *subscription, void *ros_message, //============================================================================== /// 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); @@ -1692,7 +1717,7 @@ rmw_ret_t rmw_take_with_info(const rmw_subscription_t *subscription, *taken = false; auto sub_data = - static_cast(subscription->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); @@ -1700,12 +1725,14 @@ rmw_ret_t rmw_take_with_info(const rmw_subscription_t *subscription, //============================================================================== /// 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); @@ -1737,14 +1764,14 @@ rmw_ret_t rmw_take_sequence(const rmw_subscription_t *subscription, 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)()); + (std::numeric_limits::max)()); return RMW_RET_ERROR; } *taken = 0; auto sub_data = - static_cast(subscription->data); + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); if (sub_data->context->impl->is_shutdown) { @@ -1783,10 +1810,13 @@ rmw_ret_t rmw_take_sequence(const rmw_subscription_t *subscription, } //============================================================================== -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); @@ -1801,7 +1831,7 @@ rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, *taken = false; auto sub_data = - static_cast(subscription->data); + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); if (sub_data->context->impl->is_shutdown) { @@ -1811,7 +1841,7 @@ rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, // RETRIEVE SERIALIZED MESSAGE =============================================== std::unique_ptr msg_data = - sub_data->pop_next_message(); + 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. @@ -1823,9 +1853,9 @@ rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, if (serialized_message->buffer_capacity < payload_len) { rmw_ret_t ret = - rmw_serialized_message_resize(serialized_message, payload_len); + rmw_serialized_message_resize(serialized_message, payload_len); if (ret != RMW_RET_OK) { - return ret; // Error message already set + return ret; // Error message already set } } serialized_message->buffer_length = payload_len; @@ -1840,7 +1870,7 @@ rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, // 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; + 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; @@ -1848,15 +1878,17 @@ rmw_ret_t __rmw_take_serialized(const rmw_subscription_t *subscription, 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, @@ -1866,10 +1898,11 @@ rmw_take_serialized_message(const rmw_subscription_t *subscription, //============================================================================== /// 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) { + 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, @@ -1878,9 +1911,11 @@ rmw_ret_t rmw_take_serialized_message_with_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); @@ -1891,10 +1926,12 @@ rmw_ret_t rmw_take_loaned_message(const rmw_subscription_t *subscription, //============================================================================== /// 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); @@ -1906,7 +1943,8 @@ rmw_take_loaned_message_with_info(const rmw_subscription_t *subscription, //============================================================================== /// 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) { + const rmw_subscription_t *subscription, void *loaned_message) +{ static_cast(subscription); static_cast(loaned_message); return RMW_RET_UNSUPPORTED; @@ -1915,12 +1953,11 @@ rmw_ret_t rmw_return_loaned_message_from_subscription( //============================================================================== /// 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_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); @@ -1940,7 +1977,7 @@ rmw_client_t *rmw_create_client( 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); + 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, @@ -1948,7 +1985,7 @@ rmw_client_t *rmw_create_client( RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); const rmw_zenoh_cpp::rmw_node_data_t *node_data = - static_cast(node->data); + static_cast(node->data); if (node_data == nullptr) { RMW_SET_ERROR_MSG("Unable to create client as node data is invalid."); return nullptr; @@ -1960,13 +1997,15 @@ rmw_client_t *rmw_create_client( int validation_result; if (rmw_validate_full_topic_name(service_name, &validation_result, nullptr) != - RMW_RET_OK) { + 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) { + !qos_profile->avoid_ros_namespace_conventions) + { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("service name is malformed: %s", service_name); return nullptr; @@ -1974,28 +2013,28 @@ rmw_client_t *rmw_create_client( // client data rmw_client_t *rmw_client = static_cast( - allocator->zero_allocate(1, sizeof(rmw_client_t), allocator->state)); + 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); auto free_rmw_client = rcpputils::make_scope_exit([rmw_client, allocator]() { - allocator->deallocate(rmw_client, allocator->state); + allocator->deallocate(rmw_client, allocator->state); }); auto client_data = - static_cast(allocator->allocate( + 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]() { + 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_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(client_data->~rmw_client_data_t(), rmw_zenoh_cpp::rmw_client_data_t); }); @@ -2012,18 +2051,18 @@ rmw_client_t *rmw_create_client( // Obtain the type support const rosidl_service_type_support_t *type_support = - find_service_type_support(type_supports); + 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); + 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; @@ -2033,22 +2072,22 @@ rmw_client_t *rmw_create_client( // Request type support client_data->request_type_support = - static_cast(allocator->allocate( + 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); + 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]() { + rcpputils::make_scope_exit([client_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( client_data->request_type_support->~RequestTypeSupport(), rmw_zenoh_cpp::RequestTypeSupport); @@ -2056,22 +2095,22 @@ rmw_client_t *rmw_create_client( // Response type support client_data->response_type_support = - static_cast(allocator->allocate( + 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); + 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]() { + rcpputils::make_scope_exit([client_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( client_data->response_type_support->~ResponseTypeSupport(), rmw_zenoh_cpp::ResponseTypeSupport); @@ -2083,7 +2122,7 @@ rmw_client_t *rmw_create_client( 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]() { + rcpputils::make_scope_exit([rmw_client, allocator]() { allocator->deallocate(const_cast(rmw_client->service_name), allocator->state); }); @@ -2112,7 +2151,7 @@ rmw_client_t *rmw_create_client( return nullptr; } auto free_type_hash_c_str = - rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { + rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { allocator->deallocate(type_hash_c_str, allocator->state); }); @@ -2139,7 +2178,9 @@ rmw_client_t *rmw_create_client( return nullptr; } - if (z_keyexpr_from_str(&client_data->keyexpr, client_data->entity->topic_info()->topic_keyexpr_.c_str())) { + if (z_keyexpr_from_str(&client_data->keyexpr, + client_data->entity->topic_info()->topic_keyexpr_.c_str())) + { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } @@ -2152,7 +2193,8 @@ rmw_client_t *rmw_create_client( z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL - )) { + )) + { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the client."); @@ -2175,7 +2217,8 @@ rmw_client_t *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); @@ -2190,7 +2233,7 @@ rmw_ret_t rmw_destroy_client(rmw_node_t *node, rmw_client_t *client) { rcutils_allocator_t *allocator = &node->context->options.allocator; rmw_zenoh_cpp::rmw_client_data_t *client_data = - static_cast(client->data); + static_cast(client->data); RMW_CHECK_FOR_NULL_WITH_MSG(client_data, "client implementation pointer is null.", return RMW_RET_INVALID_ARGUMENT); @@ -2223,9 +2266,10 @@ rmw_ret_t 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); @@ -2235,7 +2279,7 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); rmw_zenoh_cpp::rmw_client_data_t *client_data = - static_cast(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); @@ -2245,25 +2289,25 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, } rmw_context_impl_s *context_impl = - static_cast(client_data->context->impl); + static_cast(client_data->context->impl); // Serialize data rcutils_allocator_t *allocator = &(client_data->context->options.allocator); size_t max_data_length = - (client_data->request_type_support->get_estimated_serialized_size( + (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)); + 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]() { + rcpputils::make_scope_exit([request_bytes, allocator]() { allocator->deallocate(request_bytes, allocator->state); }); @@ -2273,7 +2317,8 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_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; } @@ -2291,7 +2336,7 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, return RMW_RET_ERROR; } auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() { z_drop(z_move(attachment)); }); + [&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. @@ -2328,9 +2373,11 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, //============================================================================== /// 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); @@ -2345,13 +2392,13 @@ rmw_ret_t rmw_take_response(const rmw_client_t *client, RMW_RET_INVALID_ARGUMENT); rmw_zenoh_cpp::rmw_client_data_t *client_data = - static_cast(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(); + 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. @@ -2369,15 +2416,16 @@ rmw_ret_t rmw_take_response(const rmw_client_t *client, // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast( - const_cast(z_slice_data(z_loan(payload)))), - z_slice_len(z_loan(payload))); + 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)) { + client_data->response_type_support_impl)) + { RMW_SET_ERROR_MSG("could not deserialize ROS response"); return RMW_RET_ERROR; } @@ -2385,7 +2433,7 @@ rmw_ret_t rmw_take_response(const rmw_client_t *client, // Fill in the request_header request_header->request_id.sequence_number = - rmw_zenoh_cpp::get_int64_from_attachment(z_sample_attachment(sample), + 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( @@ -2403,7 +2451,8 @@ rmw_ret_t rmw_take_response(const rmw_client_t *client, if (!rmw_zenoh_cpp::get_gid_from_attachment( z_sample_attachment(sample), - request_header->request_id.writer_guid)) { + request_header->request_id.writer_guid)) + { RMW_SET_ERROR_MSG("Could not get client gid from attachment"); return RMW_RET_ERROR; } @@ -2421,8 +2470,10 @@ rmw_ret_t rmw_take_response(const rmw_client_t *client, //============================================================================== /// 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, @@ -2430,7 +2481,7 @@ rmw_client_request_publisher_get_actual_qos(const rmw_client_t *client, RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); rmw_zenoh_cpp::rmw_client_data_t *client_data = - static_cast(client->data); + static_cast(client->data); RMW_CHECK_ARGUMENT_FOR_NULL(client_data, RMW_RET_INVALID_ARGUMENT); *qos = client_data->adapted_qos_profile; @@ -2440,8 +2491,10 @@ rmw_client_request_publisher_get_actual_qos(const rmw_client_t *client, //============================================================================== /// 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); } @@ -2449,9 +2502,10 @@ rmw_client_response_subscription_get_actual_qos(const rmw_client_t *client, //============================================================================== /// 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) { +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, @@ -2469,13 +2523,13 @@ rmw_service_t *rmw_create_service( // 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); + 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_full_topic_name_validation_result_string(validation_result); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "service_name argument is invalid: %s", reason); return nullptr; @@ -2483,7 +2537,7 @@ rmw_service_t *rmw_create_service( } RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); const rmw_zenoh_cpp::rmw_node_data_t *node_data = - static_cast(node->data); + static_cast(node->data); if (node_data == nullptr) { RMW_SET_ERROR_MSG("Unable to create service as node data is invalid."); return nullptr; @@ -2495,7 +2549,7 @@ rmw_service_t *rmw_create_service( 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); + 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, @@ -2505,29 +2559,29 @@ rmw_service_t *rmw_create_service( 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)); + 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]() { + rcpputils::make_scope_exit([rmw_service, allocator]() { allocator->deallocate(rmw_service, allocator->state); }); auto service_data = - static_cast(allocator->allocate( + 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]() { + 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_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(service_data->~rmw_service_data_t(), rmw_zenoh_cpp::rmw_service_data_t); }); @@ -2542,18 +2596,18 @@ rmw_service_t *rmw_create_service( // Get the RMW type support. const rosidl_service_type_support_t *type_support = - find_service_type_support(type_supports); + 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); + 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; @@ -2563,20 +2617,20 @@ rmw_service_t *rmw_create_service( // Request type support service_data->request_type_support = - static_cast(allocator->allocate( + 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); 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); + [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]() { + rcpputils::make_scope_exit([service_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( service_data->request_type_support->~RequestTypeSupport(), rmw_zenoh_cpp::RequestTypeSupport); @@ -2584,21 +2638,21 @@ rmw_service_t *rmw_create_service( // Response type support service_data->response_type_support = - static_cast(allocator->allocate( + 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); 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); + [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]() { + rcpputils::make_scope_exit([service_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( service_data->response_type_support->~ResponseTypeSupport(), rmw_zenoh_cpp::ResponseTypeSupport); @@ -2611,7 +2665,7 @@ rmw_service_t *rmw_create_service( "failed to allocate service name", return nullptr); auto free_service_name = - rcpputils::make_scope_exit([rmw_service, allocator]() { + rcpputils::make_scope_exit([rmw_service, allocator]() { allocator->deallocate(const_cast(rmw_service->service_name), allocator->state); }); @@ -2640,7 +2694,7 @@ rmw_service_t *rmw_create_service( return nullptr; } auto free_type_hash_c_str = - rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { + rcpputils::make_scope_exit([&allocator, &type_hash_c_str]() { allocator->deallocate(type_hash_c_str, allocator->state); }); @@ -2666,7 +2720,9 @@ rmw_service_t *rmw_create_service( rmw_service->service_name); return nullptr; } - if(z_keyexpr_from_str(&service_data->keyexpr, service_data->entity->topic_info()->topic_keyexpr_.c_str())) { + if(z_keyexpr_from_str(&service_data->keyexpr, + service_data->entity->topic_info()->topic_keyexpr_.c_str())) + { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } @@ -2680,12 +2736,13 @@ rmw_service_t *rmw_create_service( qable_options.complete = true; if(z_declare_queryable( &service_data->qable, z_loan(context_impl->session), - z_loan(service_data->keyexpr), z_move(callback), &qable_options)) { + z_loan(service_data->keyexpr), z_move(callback), &qable_options)) + { 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));}); std::string liveliness_keyexpr = service_data->entity->liveliness_keyexpr(); z_view_keyexpr_t liveliness_ke; @@ -2695,7 +2752,8 @@ rmw_service_t *rmw_create_service( z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL - )) { + )) + { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the service."); return nullptr; @@ -2718,7 +2776,8 @@ rmw_service_t *rmw_create_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); @@ -2733,7 +2792,7 @@ rmw_ret_t rmw_destroy_service(rmw_node_t *node, rmw_service_t *service) { rcutils_allocator_t *allocator = &node->context->options.allocator; rmw_zenoh_cpp::rmw_service_data_t *service_data = - static_cast(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); @@ -2765,10 +2824,11 @@ rmw_ret_t 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); @@ -2785,13 +2845,13 @@ rmw_ret_t rmw_take_request(const rmw_service_t *service, RMW_RET_INVALID_ARGUMENT); rmw_zenoh_cpp::rmw_service_data_t *service_data = - static_cast(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(); + 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. @@ -2807,15 +2867,16 @@ rmw_ret_t rmw_take_request(const rmw_service_t *service, // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast( - const_cast(z_slice_data(z_loan(payload)))), - z_slice_len(z_loan(payload))); + 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)) { + service_data->request_type_support_impl)) + { RMW_SET_ERROR_MSG("could not deserialize ROS message"); return RMW_RET_ERROR; } @@ -2826,7 +2887,7 @@ rmw_ret_t rmw_take_request(const rmw_service_t *service, 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"); @@ -2834,7 +2895,7 @@ rmw_ret_t rmw_take_request(const rmw_service_t *service, } request_header->source_timestamp = - rmw_zenoh_cpp::get_int64_from_attachment(attachment, "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"); @@ -2842,7 +2903,8 @@ rmw_ret_t rmw_take_request(const rmw_service_t *service, } 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; } @@ -2854,7 +2916,8 @@ rmw_ret_t rmw_take_request(const rmw_service_t *service, // 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))) { + std::move(query))) + { RMW_SET_ERROR_MSG("duplicate sequence number in the map"); return RMW_RET_ERROR; } @@ -2867,9 +2930,11 @@ rmw_ret_t rmw_take_request(const rmw_service_t *service, //============================================================================== /// 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); @@ -2884,11 +2949,11 @@ rmw_ret_t rmw_send_response(const rmw_service_t *service, RMW_RET_INVALID_ARGUMENT); rmw_zenoh_cpp::rmw_service_data_t *service_data = - static_cast(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. @@ -2898,18 +2963,18 @@ rmw_ret_t rmw_send_response(const rmw_service_t *service, rcutils_allocator_t *allocator = &(service_data->context->options.allocator); size_t max_data_length = - (service_data->response_type_support->get_estimated_serialized_size( + (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)); + 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]() { + rcpputils::make_scope_exit([response_bytes, allocator]() { allocator->deallocate(response_bytes, allocator->state); }); @@ -2920,7 +2985,8 @@ rmw_ret_t rmw_send_response(const rmw_service_t *service, 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)) { + service_data->response_type_support_impl)) + { return RMW_RET_ERROR; } @@ -2932,7 +2998,8 @@ rmw_ret_t rmw_send_response(const rmw_service_t *service, z_owned_bytes_t attachment; if (!create_map_and_set_sequence_num(&attachment, - request_header->sequence_number, request_header->writer_guid)) { + request_header->sequence_number, request_header->writer_guid)) + { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } @@ -2952,8 +3019,10 @@ rmw_ret_t rmw_send_response(const rmw_service_t *service, //============================================================================== /// 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, @@ -2961,7 +3030,7 @@ rmw_service_request_subscription_get_actual_qos(const rmw_service_t *service, RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); rmw_zenoh_cpp::rmw_service_data_t *service_data = - static_cast(service->data); + static_cast(service->data); RMW_CHECK_ARGUMENT_FOR_NULL(service_data, RMW_RET_INVALID_ARGUMENT); *qos = service_data->adapted_qos_profile; @@ -2971,30 +3040,33 @@ rmw_service_request_subscription_get_actual_qos(const rmw_service_t *service, //============================================================================== /// 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) { +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( + 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]() { + rcpputils::make_scope_exit([guard_condition, allocator]() { allocator->deallocate(guard_condition, allocator->state); }); guard_condition->implementation_identifier = - rmw_zenoh_cpp::rmw_zenoh_identifier; + rmw_zenoh_cpp::rmw_zenoh_identifier; guard_condition->context = context; guard_condition->data = allocator->zero_allocate( @@ -3003,16 +3075,16 @@ rmw_guard_condition_t *rmw_create_guard_condition(rmw_context_t *context) { 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]() { + 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]() { + rcpputils::make_scope_exit([guard_condition]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( static_cast(guard_condition->data) - ->~GuardCondition(), + ->~GuardCondition(), rmw_zenoh_cpp::GuardCondition); }); @@ -3025,14 +3097,15 @@ rmw_guard_condition_t *rmw_create_guard_condition(rmw_context_t *context) { /// 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_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; if (guard_condition->data) { static_cast(guard_condition->data) - ->~GuardCondition(); + ->~GuardCondition(); allocator->deallocate(guard_condition->data, allocator->state); } @@ -3043,7 +3116,8 @@ rmw_ret_t 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, @@ -3051,16 +3125,17 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t *guard_condition) { return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); static_cast(guard_condition->data) - ->trigger(); + ->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); @@ -3071,11 +3146,11 @@ rmw_wait_set_t *rmw_create_wait_set(rmw_context_t *context, rcutils_allocator_t *allocator = &context->options.allocator; auto wait_set = static_cast( - allocator->zero_allocate(1, sizeof(rmw_wait_set_t), 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); + allocator->deallocate(wait_set, allocator->state); }); wait_set->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; @@ -3085,20 +3160,20 @@ rmw_wait_set_t *rmw_create_wait_set(rmw_context_t *context, 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); + 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( + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( static_cast(wait_set->data) - ->~rmw_wait_set_data_t(), + ->~rmw_wait_set_data_t(), rmw_wait_set_data); }); static_cast(wait_set->data)->context = - context; + context; destruct_rmw_wait_set_data.cancel(); free_wait_set_data.cancel(); @@ -3109,7 +3184,8 @@ rmw_wait_set_t *rmw_create_wait_set(rmw_context_t *context, //============================================================================== /// 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, @@ -3118,7 +3194,7 @@ rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t *wait_set) { return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); auto wait_set_data = - static_cast(wait_set->data); + static_cast(wait_set->data); rcutils_allocator_t *allocator = &wait_set_data->context->options.allocator; @@ -3130,18 +3206,20 @@ rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t *wait_set) { return RMW_RET_OK; } -namespace { +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) { + 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]); + static_cast( + guard_conditions->guard_conditions[i]); if (gc == nullptr) { continue; } @@ -3155,7 +3233,7 @@ bool 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 " @@ -3165,13 +3243,14 @@ bool check_and_attach_condition( } auto event_data = - static_cast(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)) { + zenoh_event_type, wait_set_data)) + { return true; } } @@ -3180,7 +3259,7 @@ bool check_and_attach_condition( if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { auto sub_data = static_cast( - subscriptions->subscribers[i]); + subscriptions->subscribers[i]); if (sub_data == nullptr) { continue; } @@ -3193,12 +3272,13 @@ bool check_and_attach_condition( if (services) { for (size_t i = 0; i < services->service_count; ++i) { auto serv_data = static_cast( - services->services[i]); + services->services[i]); if (serv_data == nullptr) { continue; } if (serv_data->queue_has_data_and_attach_condition_if_not( - wait_set_data)) { + wait_set_data)) + { return true; } } @@ -3207,12 +3287,13 @@ bool 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]); + static_cast(clients->clients[i]); if (client_data == nullptr) { continue; } if (client_data->queue_has_data_and_attach_condition_if_not( - wait_set_data)) { + wait_set_data)) + { return true; } } @@ -3220,16 +3301,17 @@ bool 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_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, @@ -3237,7 +3319,7 @@ rmw_ret_t rmw_wait(rmw_subscriptions_t *subscriptions, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); auto wait_set_data = - static_cast(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); @@ -3260,7 +3342,7 @@ rmw_ret_t rmw_wait(rmw_subscriptions_t *subscriptions, // 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, + 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); @@ -3270,14 +3352,14 @@ rmw_ret_t rmw_wait(rmw_subscriptions_t *subscriptions, // 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; }); + [wait_set_data]() {return wait_set_data->triggered;}); } } @@ -3296,8 +3378,8 @@ rmw_ret_t rmw_wait(rmw_subscriptions_t *subscriptions, 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]); + static_cast( + guard_conditions->guard_conditions[i]); if (gc == nullptr) { continue; } @@ -3315,19 +3397,20 @@ rmw_ret_t rmw_wait(rmw_subscriptions_t *subscriptions, for (size_t i = 0; i < events->event_count; ++i) { auto event = static_cast(events->events[i]); auto event_data = - static_cast(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)) { + zenoh_event_type)) + { // Setting to nullptr lets rcl know that this subscription is not ready events->events[i] = nullptr; } else { @@ -3339,7 +3422,7 @@ rmw_ret_t rmw_wait(rmw_subscriptions_t *subscriptions, if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { auto sub_data = static_cast( - subscriptions->subscribers[i]); + subscriptions->subscribers[i]); if (sub_data == nullptr) { continue; } @@ -3356,7 +3439,7 @@ rmw_ret_t rmw_wait(rmw_subscriptions_t *subscriptions, if (services) { for (size_t i = 0; i < services->service_count; ++i) { auto serv_data = static_cast( - services->services[i]); + services->services[i]); if (serv_data == nullptr) { continue; } @@ -3373,7 +3456,7 @@ rmw_ret_t rmw_wait(rmw_subscriptions_t *subscriptions, 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]); + static_cast(clients->clients[i]); if (client_data == nullptr) { continue; } @@ -3392,10 +3475,11 @@ rmw_ret_t rmw_wait(rmw_subscriptions_t *subscriptions, //============================================================================== /// 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); @@ -3412,9 +3496,9 @@ rmw_ret_t rmw_get_node_names(const rmw_node_t *node, //============================================================================== /// 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) { - + 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); @@ -3431,8 +3515,10 @@ rmw_ret_t rmw_get_node_names_with_enclaves( //============================================================================== /// 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, @@ -3440,13 +3526,13 @@ rmw_ret_t rmw_count_publishers(const rmw_node_t *node, const char *topic_name, 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_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_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; @@ -3458,8 +3544,10 @@ rmw_ret_t rmw_count_publishers(const rmw_node_t *node, const char *topic_name, //============================================================================== /// 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, @@ -3467,13 +3555,13 @@ rmw_ret_t rmw_count_subscribers(const rmw_node_t *node, const char *topic_name, 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_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_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; @@ -3486,8 +3574,10 @@ rmw_ret_t rmw_count_subscribers(const rmw_node_t *node, const char *topic_name, //============================================================================== /// 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, @@ -3495,13 +3585,13 @@ rmw_ret_t rmw_count_clients(const rmw_node_t *node, const char *service_name, 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_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_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; @@ -3513,8 +3603,10 @@ rmw_ret_t rmw_count_clients(const rmw_node_t *node, const char *service_name, //============================================================================== /// 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, @@ -3522,13 +3614,13 @@ rmw_ret_t rmw_count_services(const rmw_node_t *node, const char *service_name, 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_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_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; @@ -3540,13 +3632,15 @@ rmw_ret_t rmw_count_services(const rmw_node_t *node, const char *service_name, //============================================================================== /// 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); + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; @@ -3557,12 +3651,13 @@ rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t *publisher, //============================================================================== /// 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); + static_cast(client->data); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; memcpy(gid->data, client_data->client_gid, RMW_GID_STORAGE_SIZE); @@ -3572,8 +3667,10 @@ rmw_ret_t 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, @@ -3591,9 +3688,11 @@ rmw_ret_t rmw_compare_gids_equal(const rmw_gid_t *gid1, const rmw_gid_t *gid2, //============================================================================== /// 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, @@ -3603,7 +3702,7 @@ rmw_ret_t rmw_service_server_is_available(const rmw_node_t *node, RMW_CHECK_ARGUMENT_FOR_NULL(is_available, RMW_RET_INVALID_ARGUMENT); rmw_zenoh_cpp::rmw_client_data_t *client_data = - static_cast(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", @@ -3628,25 +3727,26 @@ rmw_ret_t rmw_service_server_is_available(const rmw_node_t *node, //============================================================================== /// 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; } @@ -3654,12 +3754,14 @@ rmw_ret_t 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); + 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); return RMW_RET_OK; @@ -3667,12 +3769,14 @@ rmw_subscription_set_on_new_message_callback(rmw_subscription_t *subscription, //============================================================================== /// 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); + 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); return RMW_RET_OK; @@ -3680,14 +3784,16 @@ rmw_ret_t rmw_service_set_on_new_request_callback(rmw_service_t *service, //============================================================================== /// 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); + 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); return RMW_RET_OK; } -} // extern "C" +} // extern "C" From 7dcc7d62568fe4b93786a756263e7caf964583b6 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 21:15:49 +0800 Subject: [PATCH 33/77] ci: fix the argument order in the style CI --- .github/workflows/style.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index 70b74e75..b7d1696d 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -22,6 +22,6 @@ jobs: steps: - uses: actions/checkout@v4 - name: uncrustify - run: /ros_entrypoint.sh ament_uncrustify --exclude rmw_zenoh_cpp/src/detail/ordered_hash.hpp rmw_zenoh_cpp/src/detail/ordered_map.hpp rmw_zenoh_cpp/ + run: /ros_entrypoint.sh ament_uncrustify rmw_zenoh_cpp/ --exclude rmw_zenoh_cpp/src/detail/ordered_hash.hpp rmw_zenoh_cpp/src/detail/ordered_map.hpp - name: cpplint - run: /ros_entrypoint.sh ament_cpplint --exclude rmw_zenoh_cpp/src/detail/ordered_hash.hpp rmw_zenoh_cpp/src/detail/ordered_map.hpp rmw_zenoh_cpp/ + run: /ros_entrypoint.sh ament_cpplint rmw_zenoh_cpp/ --exclude rmw_zenoh_cpp/src/detail/ordered_hash.hpp rmw_zenoh_cpp/src/detail/ordered_map.hpp From 7bbfdf50d4968913c61c355c19c2fdb7eedd58c0 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 21:17:39 +0800 Subject: [PATCH 34/77] chore: remove .clang_format --- .clang_format | 20 -------------------- 1 file changed, 20 deletions(-) delete mode 100644 .clang_format diff --git a/.clang_format b/.clang_format deleted file mode 100644 index 4a79087d..00000000 --- a/.clang_format +++ /dev/null @@ -1,20 +0,0 @@ ---- -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 -... From ed51a81a6cd8f211ce9f843620aa2275f8489ca3 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 21:21:02 +0800 Subject: [PATCH 35/77] chore: clean up --- .github/workflows/build.yaml | 5 +---- .github/workflows/check_logging.yaml | 5 +---- .github/workflows/style.yaml | 5 +---- .gitignore | 4 ---- 4 files changed, 3 insertions(+), 16 deletions(-) delete mode 100644 .gitignore diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 1d5e5538..8829ee50 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -28,10 +28,7 @@ jobs: BUILD_TYPE: binary env: ROS2_REPOS_FILE_URL: 'https://raw.githubusercontent.com/ros2/ros2/${{ matrix.ROS_DISTRO }}/ros2.repos' - runs-on: - - self-hosted - - X64 - - ubuntu-ros + runs-on: ubuntu-latest container: image: ${{ matrix.BUILD_TYPE == 'binary' && format('ros:{0}-ros-base', matrix.ROS_DISTRO) || 'ubuntu:noble' }} steps: diff --git a/.github/workflows/check_logging.yaml b/.github/workflows/check_logging.yaml index 453f90ab..caffa805 100644 --- a/.github/workflows/check_logging.yaml +++ b/.github/workflows/check_logging.yaml @@ -4,10 +4,7 @@ name: "Check logging macros" on: [pull_request] jobs: check_logging: - runs-on: - - self-hosted - - X64 - - ubuntu-ros + runs-on: ubuntu-latest steps: - name: Check logging macros uses: JJ/github-pr-contains-action@releases/v14.1 diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index b7d1696d..1581e8c7 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -8,10 +8,7 @@ defaults: shell: bash jobs: test: - runs-on: - - self-hosted - - X64 - - ubuntu-ros + runs-on: ubuntu-latest strategy: fail-fast: false matrix: diff --git a/.gitignore b/.gitignore deleted file mode 100644 index 01716761..00000000 --- a/.gitignore +++ /dev/null @@ -1,4 +0,0 @@ -build -install -.cache -log From 975ea82fbc722c80d6751247a9c03bfc54f3cf90 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 21:31:36 +0800 Subject: [PATCH 36/77] refactor: use `z_id_to_string` --- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index 16bda1ee..4ad19def 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -13,13 +13,11 @@ // limitations under the License. #include "liveliness_utils.hpp" +#include #include -#include #include -#include #include -#include #include #include #include @@ -27,8 +25,6 @@ #include "logging_macros.hpp" #include "qos.hpp" -#include "rcpputils/scope_exit.hpp" - #include "rmw/error_handling.h" namespace rmw_zenoh_cpp @@ -371,11 +367,11 @@ std::optional keyexpr_to_qos(const std::string & keyexpr) ///============================================================================= std::string zid_to_str(const z_id_t & id) { - std::ostringstream oss; - for (int i = sizeof(id.id) - 1; i >= 0; i--) { - oss << std::setw(2) << std::setfill('0') << std::hex << static_cast(id.id[i]); - } - return oss.str(); + z_owned_string_t z_str; + z_id_to_string(&id, &z_str); + std::string str(z_string_data(z_loan(z_str)), z_string_len(z_loan(z_str))); + z_drop(z_move(z_str)); + return str; } ///============================================================================= From 17adcfa62ea86d1e2500abf06f80acb52243be6f Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 21:36:27 +0800 Subject: [PATCH 37/77] chore: tidy up --- rmw_zenoh_cpp/src/detail/logging.cpp | 1 - rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 1 - 2 files changed, 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/logging.cpp b/rmw_zenoh_cpp/src/detail/logging.cpp index 2021f90f..1977b0f2 100644 --- a/rmw_zenoh_cpp/src/detail/logging.cpp +++ b/rmw_zenoh_cpp/src/detail/logging.cpp @@ -63,5 +63,4 @@ void Logger::log_named( ); } } - } // 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 b6744971..7f3dbe6e 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -52,7 +52,6 @@ class rmw_context_impl_s final // An owned session. z_owned_session_t session; - // TODO(yuyuan): SHM std::optional shm_provider; z_owned_subscriber_t graph_subscriber; From e62c9bd15aa132ef602e15eeba81ca21838b2c0b Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 21:53:29 +0800 Subject: [PATCH 38/77] build: use the latest zenoh-c commit --- zenoh_c_vendor/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 03f8bd26..9cf4a993 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -25,8 +25,8 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - 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 - VCS_VERSION main + VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git + VCS_VERSION 5d00adfd694bea55069b640ece71895ed182e61b CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" From 458414dfd7d7b7f13e6547cf79c5c2045d03aac4 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 22:33:43 +0800 Subject: [PATCH 39/77] build: enable the unstable feature flag --- zenoh_c_vendor/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 9cf4a993..ef90d646 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -29,6 +29,7 @@ ament_vendor(zenoh_c_vendor VCS_VERSION 5d00adfd694bea55069b640ece71895ed182e61b CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" + "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" ) From bf4977593741b8cae6fd0d6d91816f7c976e8c1f Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 22:34:12 +0800 Subject: [PATCH 40/77] chore: make iron ament_uncrustify happy --- .../src/detail/attachment_helpers.cpp | 20 +- .../src/detail/attachment_helpers.hpp | 12 +- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 4 +- rmw_zenoh_cpp/src/detail/logging_macros.hpp | 10 +- rmw_zenoh_cpp/src/detail/qos.hpp | 2 +- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 122 +- .../src/detail/zenoh_router_check.cpp | 8 +- rmw_zenoh_cpp/src/rmw_init.cpp | 206 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 2105 +++++++++-------- 9 files changed, 1362 insertions(+), 1127 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index c0e262f1..8ce5348b 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -27,9 +27,9 @@ namespace rmw_zenoh_cpp { -bool create_attachment_iter(z_owned_bytes_t *kv_pair, void *context) +bool create_attachment_iter(z_owned_bytes_t * kv_pair, void * context) { - attachement_context_t *ctx = reinterpret_cast(context); + attachement_context_t * ctx = reinterpret_cast(context); z_owned_bytes_t k, v; if (ctx->idx == 0) { @@ -40,8 +40,9 @@ 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_buf(&v, ctx->data->source_gid, - RMW_GID_STORAGE_SIZE); + z_bytes_serialize_from_buf( + &v, ctx->data->source_gid, + RMW_GID_STORAGE_SIZE); } else { return false; } @@ -51,16 +52,17 @@ bool create_attachment_iter(z_owned_bytes_t *kv_pair, void *context) return true; } -z_result_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_from_iter(attachment, create_attachment_iter, - reinterpret_cast(&context)); + return z_bytes_from_iter( + attachment, create_attachment_iter, + reinterpret_cast(&context)); } bool get_attachment( const z_loaned_bytes_t *const attachment, - const std::string & key, z_owned_bytes_t *val) + const std::string & key, z_owned_bytes_t * val) { if (z_bytes_is_empty(attachment)) { return false; @@ -141,7 +143,7 @@ int64_t get_int64_from_attachment( 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.") + "rmw_zenoh_cpp", "Failed to deserialize int64 from the attachment.") return false; } diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp index aa44de43..2d663706 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -24,7 +24,8 @@ namespace rmw_zenoh_cpp { -class attachement_data_t final { +class attachement_data_t final +{ public: int64_t sequence_number; int64_t source_timestamp; @@ -41,19 +42,20 @@ class attachement_data_t final { z_result_t serialize_to_zbytes(z_owned_bytes_t *); }; -class attachement_context_t final { +class attachement_context_t final +{ public: - const attachement_data_t *data; + const attachement_data_t * data; int length = 3; int idx = 0; - attachement_context_t(const attachement_data_t *_data) + 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); + const std::string & key, z_owned_bytes_t * val); bool get_gid_from_attachment( const z_loaned_bytes_t *const attachment, diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index ceccbe82..e5928237 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -696,8 +696,8 @@ rmw_ret_t GraphCache::get_node_names( rcutils_ret_t ret = rcutils_string_array_fini(enclaves); if (ret != RCUTILS_RET_OK) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "failed to cleanup during error handling: %s", rcutils_get_error_string().str); + "rmw_zenoh_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string().str); } }; diff --git a/rmw_zenoh_cpp/src/detail/logging_macros.hpp b/rmw_zenoh_cpp/src/detail/logging_macros.hpp index 81201856..44a4bc34 100644 --- a/rmw_zenoh_cpp/src/detail/logging_macros.hpp +++ b/rmw_zenoh_cpp/src/detail/logging_macros.hpp @@ -24,14 +24,14 @@ // invoke GraphCache::parse_put() and GraphCache::parse_del() functions. // See https://github.com/ros2/rmw_zenoh/issues/182 for more details. #define RMW_ZENOH_LOG_DEBUG_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_DEBUG, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_DEBUG, __func__, __FILE__, __LINE__, __VA_ARGS__);} #define RMW_ZENOH_LOG_ERROR_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_ERROR, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_ERROR, __func__, __FILE__, __LINE__, __VA_ARGS__);} #define RMW_ZENOH_LOG_FATAL_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_FATAL, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_FATAL, __func__, __FILE__, __LINE__, __VA_ARGS__);} #define RMW_ZENOH_LOG_INFO_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_INFO, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_INFO, __func__, __FILE__, __LINE__, __VA_ARGS__);} #define RMW_ZENOH_LOG_WARN_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_WARN, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_WARN, __func__, __FILE__, __LINE__, __VA_ARGS__);} #endif // DETAIL__LOGGING_MACROS_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/qos.hpp b/rmw_zenoh_cpp/src/detail/qos.hpp index eee74c05..6b8e9230 100644 --- a/rmw_zenoh_cpp/src/detail/qos.hpp +++ b/rmw_zenoh_cpp/src/detail/qos.hpp @@ -25,7 +25,7 @@ namespace rmw_zenoh_cpp { //============================================================================== /// Signature matching rmw_get_publishers_info_by_topic and rmw_get_subscriptions_info_by_topic -using GetEndpointInfoByTopicFunction = std::function lock(condition_mutex_); if (!message_queue_.empty()) { @@ -141,10 +141,10 @@ void rmw_subscription_data_t::add_new_message( { // 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()); + "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 @@ -169,8 +169,9 @@ void rmw_subscription_data_t::add_new_message( 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 @@ -186,7 +187,7 @@ void rmw_subscription_data_t::add_new_message( ///============================================================================= 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()) { @@ -243,10 +244,10 @@ void rmw_service_data_t::add_new_query(std::unique_ptr query) 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)); @@ -284,7 +285,7 @@ bool rmw_service_data_t::add_to_query_map( } it->second.insert( - std::make_pair(request_id.sequence_number, std::move(query))); + std::make_pair(request_id.sequence_number, std::move(query))); return true; } @@ -343,10 +344,10 @@ void rmw_client_data_t::add_new_reply(std::unique_ptr reply) 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)); @@ -359,7 +360,7 @@ void rmw_client_data_t::add_new_reply(std::unique_ptr reply) ///============================================================================= 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()) { @@ -432,7 +433,7 @@ bool rmw_client_data_t::is_shutdown() const } //============================================================================== -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); @@ -440,21 +441,21 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) 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 = @@ -464,8 +465,8 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) // 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 = @@ -475,24 +476,24 @@ void sub_data_handler(const z_loaned_sample_t *sample, void *data) // 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."); } - const z_loaned_bytes_t *payload = z_sample_payload(sample); + const z_loaned_bytes_t * payload = z_sample_payload(sample); z_owned_slice_t slice; z_bytes_deserialize_into_slice(payload, &slice); sub_data->add_new_message( - std::make_unique( - slice, z_timestamp_ntp64_time(z_sample_timestamp(sample)), pub_gid, - sequence_number, source_timestamp), - z_string_data(z_loan(keystr))); + std::make_unique( + slice, 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) +ZenohQuery::ZenohQuery(const z_loaned_query_t * query) { z_query_clone(&query_, query); } @@ -507,18 +508,18 @@ const z_loaned_query_t * ZenohQuery::get_query() const } //============================================================================== -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; } @@ -526,7 +527,7 @@ 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_));} @@ -547,12 +548,13 @@ size_t rmw_client_data_t::get_next_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; } @@ -569,28 +571,29 @@ void client_data_handler(const z_loaned_reply_t *reply, void *data) } 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); - 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)), static_cast(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)), static_cast(z_string_len(z_loan(err_str))), + z_string_data(z_loan(err_str))); z_drop(z_move(err_str)); return; } } -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; } @@ -598,14 +601,15 @@ void client_data_drop(void *data) // 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); + 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); } } } diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp index 74081fb9..f837e6ba 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp @@ -33,10 +33,10 @@ rmw_ret_t zenoh_router_check(const z_loaned_session_t * session) auto callback = [](const struct z_id_t * id, void * ctx) { const std::string id_str = liveliness::zid_to_str(*id); RMW_ZENOH_LOG_INFO_NAMED( - "rmw_zenoh_cpp", - "Successfully connected to a Zenoh router with id %s.", id_str.c_str()); - // Note: Callback is guaranteed to never be called - // concurrently according to z_info_routers_zid docstring + "rmw_zenoh_cpp", + "Successfully connected to a Zenoh router with id %s.", id_str.c_str()); + // Note: Callback is guaranteed to never be called + // concurrently according to z_info_routers_zid docstring (*(static_cast(ctx)))++; }; diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 2a6f6981..e12ef24b 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -54,7 +54,7 @@ graph_sub_data_handler(const z_loaned_sample_t * sample, void * data) 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", @@ -89,18 +89,21 @@ 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) +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; @@ -116,44 +119,52 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) context->actual_domain_id = RMW_DEFAULT_DOMAIN_ID != options->domain_id ? options->domain_id : 0u; - const rcutils_allocator_t *allocator = &options->allocator; + 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_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( + 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; @@ -169,7 +180,7 @@ rmw_ret_t 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_zenoh_cpp::ConfigurableEntity::Session, &config)) != RMW_RET_OK) { RMW_SET_ERROR_MSG("Error configuring Zenoh session."); @@ -186,7 +197,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) }); // Initialize the zenoh session. - if(z_open(&context->impl->session, z_move(config))) { + if (z_open(&context->impl->session, z_move(config))) { RMW_SET_ERROR_MSG("Error setting up zenoh session"); return RMW_RET_ERROR; } @@ -209,7 +220,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) connection_attempts < configured_connection_attempts.value()) { if ((ret = rmw_zenoh_cpp::zenoh_router_check( - z_loan(context->impl->session))) != RMW_RET_OK) + z_loan(context->impl->session))) != RMW_RET_OK) { ++connection_attempts; } @@ -217,8 +228,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) } 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; } } @@ -252,40 +263,49 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) // 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); + 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); - }); + 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); + 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); - }); + 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 = @@ -315,8 +335,9 @@ rmw_ret_t rmw_init(const rmw_init_options_t *options, rmw_context_t *context) 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); + 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) == Z_OK) { @@ -355,7 +376,7 @@ 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()); - if(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) != Z_OK) @@ -381,14 +402,16 @@ rmw_ret_t 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_provider.has_value()) { @@ -407,35 +430,40 @@ rmw_ret_t 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( + 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_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); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index c89da184..a5aff757 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -65,16 +65,16 @@ const rosidl_message_type_support_t * find_message_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_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; } } @@ -84,25 +84,26 @@ 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) +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 * 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; } } @@ -151,17 +152,20 @@ 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_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; @@ -173,7 +177,7 @@ rmw_node_t * rmw_create_node( return nullptr; } if (RMW_NODE_NAME_VALID != validation_result) { - const char *reason = + 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; @@ -185,54 +189,62 @@ rmw_node_t * rmw_create_node( return nullptr; } if (RMW_NAMESPACE_VALID != validation_result) { - const char *reason = + 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( + 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_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->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(allocator->allocate( - sizeof(rmw_zenoh_cpp::rmw_node_data_t), allocator->state)); + 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); - }); + 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); - }); + 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. @@ -246,8 +258,8 @@ rmw_node_t * rmw_create_node( 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; } @@ -255,16 +267,18 @@ rmw_node_t * rmw_create_node( z_view_keyexpr_t keyexpr; if (z_view_keyexpr_from_str(&keyexpr, liveliness_keyexpr.c_str())) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Unable to generate keyexpr from the entity string."); + "rmw_zenoh_cpp", "Unable to generate keyexpr from the entity string."); return nullptr; } - if (zc_liveliness_declare_token(&node_data->token, - z_loan(context->impl->session), - z_loan(keyexpr), NULL)) + 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."); + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Failed to declare liveness token."); return nullptr; } @@ -287,21 +301,22 @@ rmw_node_t * 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) +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; + 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 = + 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)); @@ -319,12 +334,13 @@ rmw_ret_t 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); @@ -335,9 +351,9 @@ 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) + 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); @@ -349,7 +365,7 @@ rmw_ret_t 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"); @@ -375,14 +391,15 @@ void generate_random_gid(uint8_t gid[RMW_GID_STORAGE_SIZE]) //============================================================================== /// 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) + 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') { @@ -398,7 +415,7 @@ rmw_publisher_t * rmw_create_publisher( return nullptr; } if (RMW_TOPIC_VALID != validation_result) { - const char *reason = + 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; @@ -408,74 +425,85 @@ rmw_publisher_t * rmw_create_publisher( 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_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 = + 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 = + 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->impl, "expected initialized context impl", return nullptr); - rmw_context_impl_s *context_impl = + 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); + 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); - 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); + 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); - }); + 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); + 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( + 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; } @@ -487,24 +515,28 @@ rmw_publisher_t * rmw_create_publisher( 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); + 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); - }); + 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); + 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); - }); + 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; @@ -514,27 +546,31 @@ rmw_publisher_t * rmw_create_publisher( 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); + 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); - }); + 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); - }); + rcpputils::make_scope_exit( + [&allocator, &type_hash_c_str]() { + allocator->deallocate(type_hash_c_str, allocator->state); + }); const z_id_t zid = z_info_zid(z_loan(node->context->impl->session)); @@ -585,11 +621,11 @@ rmw_publisher_t * rmw_create_publisher( pub_cache_opts.queryable_prefix = z_loan(prefix_ke); ze_owned_publication_cache_t pub_cache; - if(ze_declare_publication_cache( - &pub_cache, - z_loan(context_impl->session), - z_loan(pub_ke), - &pub_cache_opts + if (ze_declare_publication_cache( + &pub_cache, + z_loan(context_impl->session), + z_loan(pub_ke), + &pub_cache_opts )) { RMW_SET_ERROR_MSG("unable to create zenoh publisher cache"); @@ -598,11 +634,12 @@ rmw_publisher_t * rmw_create_publisher( 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())); - } - }); + 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; @@ -617,29 +654,31 @@ rmw_publisher_t * rmw_create_publisher( } // 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(pub_ke), &opts) < 0) + if (z_declare_publisher( + &publisher_data->pub, z_loan(context_impl->session), + z_loan(pub_ke), &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)); + }); std::string liveliness_keyexpr = publisher_data->entity->liveliness_keyexpr(); z_view_keyexpr_t liveliness_ke; z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); - if(zc_liveliness_declare_token( - &publisher_data->token, - z_loan(node->context->impl->session), - z_loan(liveliness_ke), - NULL + if (zc_liveliness_declare_token( + &publisher_data->token, + z_loan(node->context->impl->session), + z_loan(liveliness_ke), + NULL )) { 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; } @@ -658,22 +697,24 @@ rmw_publisher_t * 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) +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); @@ -682,19 +723,22 @@ rmw_ret_t rmw_destroy_publisher(rmw_node_t *node, rmw_publisher_t *publisher) 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; @@ -702,9 +746,9 @@ rmw_ret_t rmw_destroy_publisher(rmw_node_t *node, rmw_publisher_t *publisher) //============================================================================== 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) + 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); @@ -715,10 +759,10 @@ rmw_ret_t 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) + 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); @@ -730,8 +774,8 @@ rmw_ret_t 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) + 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); @@ -743,9 +787,9 @@ rmw_ret_t 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) + const rmw_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + void ** ros_message) { static_cast(publisher); static_cast(type_support); @@ -757,8 +801,8 @@ 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) + const rmw_publisher_t * publisher, + void * loaned_message) { static_cast(publisher); static_cast(loaned_message); @@ -780,8 +824,9 @@ create_map_and_set_sequence_num( 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"); + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Failed to serialize the attachment"); return false; } @@ -792,49 +837,55 @@ create_map_and_set_sequence_num( //============================================================================== /// Publish a ROS message. rmw_ret_t rmw_publish( - const rmw_publisher_t *publisher, const void *ros_message, - rmw_publisher_allocation_t *allocation) + 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_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); + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher_data, "publisher_data is null", + return RMW_RET_INVALID_ARGUMENT); - rcutils_allocator_t *allocator = + 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); + ros_message, publisher_data->type_support_impl); // To store serialized message byte array. - char *msg_bytes = nullptr; + char * msg_bytes = nullptr; // 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 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); - } - }); + 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. @@ -846,8 +897,8 @@ rmw_ret_t rmw_publish( // 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); + &alloc, z_loan(provider), + SHM_BUF_OK_SIZE, alignment); if (alloc.status == ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { shmbuf = std::make_optional(alloc.buf); @@ -855,7 +906,7 @@ rmw_ret_t rmw_publish( 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."); + "rmw_zenoh_cpp", "Unexpected failure during SHM buffer allocation."); return RMW_RET_ERROR; } @@ -865,8 +916,9 @@ rmw_ret_t rmw_publish( // 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); + RMW_CHECK_FOR_NULL_WITH_MSG( + msg_bytes, "bytes for message is null", + return RMW_RET_BAD_ALLOC); } // Object that manages the raw buffer @@ -875,7 +927,7 @@ rmw_ret_t 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; @@ -908,7 +960,7 @@ rmw_ret_t rmw_publish( 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); + &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; @@ -921,9 +973,9 @@ rmw_ret_t 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) + const rmw_publisher_t * publisher, + void * ros_message, + rmw_publisher_allocation_t * allocation) { static_cast(publisher); static_cast(ros_message); @@ -935,43 +987,45 @@ rmw_ret_t 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) + 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 = + 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 = + 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) + 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 = + rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); @@ -982,25 +1036,28 @@ rmw_ret_t 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) + 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); + 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), @@ -1044,8 +1101,8 @@ rmw_ret_t 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) + 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); @@ -1056,13 +1113,15 @@ rmw_ret_t 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_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 = + 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); @@ -1073,7 +1132,7 @@ rmw_ret_t 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, + const rmw_publisher_t * publisher, rmw_time_t wait_timeout) { static_cast(publisher); @@ -1091,11 +1150,11 @@ rmw_ret_t rmw_publisher_wait_for_all_acked( //============================================================================== /// 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 void * ros_message, + const rosidl_message_type_support_t * type_support, + rmw_serialized_message_t * serialized_message) { - const rosidl_message_type_support_t *ts = + 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 @@ -1128,11 +1187,11 @@ rmw_ret_t 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 rmw_serialized_message_t * serialized_message, + const rosidl_message_type_support_t * type_support, + void * ros_message) { - const rosidl_message_type_support_t *ts = + 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 @@ -1155,9 +1214,9 @@ rmw_ret_t rmw_deserialize( //============================================================================== /// 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) + 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); @@ -1169,7 +1228,7 @@ rmw_ret_t 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; @@ -1178,14 +1237,15 @@ rmw_fini_subscription_allocation(rmw_subscription_allocation_t *allocation) //============================================================================== /// 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) + 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') { @@ -1195,7 +1255,7 @@ rmw_subscription_t * 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 = + 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 @@ -1205,60 +1265,70 @@ rmw_subscription_t * 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); + 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 = + 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); + 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); - 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 = + 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); + 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); - }); + 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( + 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( 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; @@ -1271,22 +1341,26 @@ rmw_subscription_t * rmw_create_subscription( 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( + 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( sub_data->type_support->~MessageTypeSupport(), rmw_zenoh_cpp::MessageTypeSupport); - }); + }); sub_data->context = node->context; @@ -1294,13 +1368,16 @@ rmw_subscription_t * rmw_create_subscription( 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); + 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); - }); + 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; @@ -1308,17 +1385,18 @@ rmw_subscription_t * 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); - }); + 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. @@ -1382,9 +1460,10 @@ rmw_subscription_t * rmw_create_subscription( } ze_owned_querying_subscriber_t sub; - if (ze_declare_querying_subscriber(&sub, z_loan(context_impl->session), - z_loan(ke), z_move(callback), - &sub_options)) + if (ze_declare_querying_subscriber( + &sub, z_loan(context_impl->session), + z_loan(ke), z_move(callback), + &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; @@ -1430,7 +1509,8 @@ rmw_subscription_t * rmw_create_subscription( } z_owned_subscriber_t sub; - if(z_declare_subscriber(&sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), + if (z_declare_subscriber( + &sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); @@ -1439,36 +1519,37 @@ rmw_subscription_t * rmw_create_subscription( sub_data->sub = 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(z_move(*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(z_move(*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(z_move(*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(z_move(*querying_sub))) - { - RMW_SET_ERROR_MSG("failed to undeclare sub"); - } } - }); + } + }); // Publish to the graph that a new subscription is in town. std::string liveliness_keyexpr = sub_data->entity->liveliness_keyexpr(); z_view_keyexpr_t liveliness_ke; z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); if (zc_liveliness_declare_token( - &sub_data->token, - z_loan(context_impl->session), - z_loan(liveliness_ke), - NULL + &sub_data->token, + z_loan(context_impl->session), + z_loan(liveliness_ke), + NULL )) { 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; } @@ -1489,22 +1570,24 @@ rmw_subscription_t * 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) + 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); @@ -1512,11 +1595,12 @@ rmw_ret_t rmw_destroy_subscription( // 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 = + z_owned_subscriber_t * sub = std::get_if(&sub_data->sub); if (sub != nullptr) { if (z_undeclare_subscriber(z_move(*sub))) { @@ -1524,7 +1608,7 @@ rmw_ret_t rmw_destroy_subscription( ret = RMW_RET_ERROR; } } else { - ze_owned_querying_subscriber_t *querying_sub = + ze_owned_querying_subscriber_t * querying_sub = std::get_if(&sub_data->sub); if (querying_sub == nullptr || ze_undeclare_querying_subscriber(z_move(*querying_sub))) @@ -1534,12 +1618,14 @@ rmw_ret_t rmw_destroy_subscription( } } - 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; @@ -1548,40 +1634,42 @@ rmw_ret_t rmw_destroy_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) + 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 = + 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 = + 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) + 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 = @@ -1595,8 +1683,8 @@ 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_subscription_t * subscription, + const rmw_subscription_content_filter_options_t * options) { static_cast(subscription); static_cast(options); @@ -1606,8 +1694,8 @@ rmw_ret_t 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) + const rmw_subscription_t * subscription, rcutils_allocator_t * allocator, + rmw_subscription_content_filter_options_t * options) { static_cast(subscription); static_cast(allocator); @@ -1618,9 +1706,9 @@ rmw_ret_t rmw_subscription_get_content_filter( 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) + rmw_zenoh_cpp::rmw_subscription_data_t * sub_data, + void * ros_message, rmw_message_info_t * message_info, + bool * taken) { *taken = false; @@ -1631,7 +1719,7 @@ rmw_ret_t __rmw_take_one( 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 @@ -1641,7 +1729,7 @@ rmw_ret_t __rmw_take_one( // 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; @@ -1655,8 +1743,9 @@ rmw_ret_t __rmw_take_one( 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); + memcpy( + message_info->publisher_gid.data, msg_data->publisher_gid, + RMW_GID_STORAGE_SIZE); message_info->from_intra_process = false; } @@ -1669,8 +1758,8 @@ rmw_ret_t __rmw_take_one( //============================================================================== /// 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) + const rmw_subscription_t * subscription, void * ros_message, + bool * taken, rmw_subscription_allocation_t * allocation) { static_cast(allocation); @@ -1679,10 +1768,11 @@ rmw_ret_t 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; @@ -1696,10 +1786,10 @@ rmw_ret_t 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) + const rmw_subscription_t * subscription, + void * ros_message, bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { static_cast(allocation); @@ -1709,10 +1799,11 @@ rmw_ret_t 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; @@ -1726,12 +1817,12 @@ rmw_ret_t rmw_take_with_info( //============================================================================== /// Take multiple incoming ROS messages with their metadata. rmw_ret_t rmw_take_sequence( - const rmw_subscription_t *subscription, + 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_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t * taken, + rmw_subscription_allocation_t * allocation) { static_cast(allocation); @@ -1741,10 +1832,11 @@ rmw_ret_t 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"); @@ -1763,7 +1855,7 @@ rmw_ret_t 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, + "Cannot take %zu samples at once, limit is %" PRIu32, count, (std::numeric_limits::max)()); return RMW_RET_ERROR; } @@ -1783,8 +1875,9 @@ rmw_ret_t 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 @@ -1813,9 +1906,9 @@ rmw_ret_t 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) + 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); @@ -1823,10 +1916,11 @@ rmw_ret_t __rmw_take_serialized( 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; @@ -1848,7 +1942,7 @@ rmw_ret_t __rmw_take_serialized( 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) { @@ -1871,8 +1965,9 @@ rmw_ret_t __rmw_take_serialized( 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); + memcpy( + message_info->publisher_gid.data, msg_data->publisher_gid, + RMW_GID_STORAGE_SIZE); message_info->from_intra_process = false; } @@ -1884,37 +1979,39 @@ rmw_ret_t __rmw_take_serialized( /// 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) + 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) + 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) + const rmw_subscription_t * subscription, + void ** loaned_message, bool * taken, + rmw_subscription_allocation_t * allocation) { static_cast(subscription); static_cast(loaned_message); @@ -1927,10 +2024,10 @@ rmw_ret_t 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) + 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); @@ -1943,7 +2040,7 @@ 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) + const rmw_subscription_t * subscription, void * loaned_message) { static_cast(subscription); static_cast(loaned_message); @@ -1954,13 +2051,14 @@ rmw_ret_t rmw_return_loaned_message_from_subscription( /// 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) + 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) { @@ -1970,28 +2068,32 @@ rmw_client_t * 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->impl, "expected initialized context impl", return nullptr); - rmw_context_impl_s *context_impl = + 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); + 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); RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - const rmw_zenoh_cpp::rmw_node_data_t *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."); return nullptr; } - rcutils_allocator_t *allocator = &node->context->options.allocator; + rcutils_allocator_t * allocator = &node->context->options.allocator; // Validate service name int validation_result; @@ -2006,51 +2108,57 @@ rmw_client_t * rmw_create_client( 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); + 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( + 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)); + 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); + 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); - }); + 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); - }); + 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 = + 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 @@ -2073,59 +2181,68 @@ rmw_client_t * rmw_create_client( // Request type support client_data->request_type_support = static_cast(allocator->allocate( - sizeof(rmw_zenoh_cpp::RequestTypeSupport), allocator->state)); + 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); - }); + 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); + 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); - }); + 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)); + 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); - }); + 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); + 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); - }); + 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); + 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); - }); + 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 @@ -2136,24 +2253,25 @@ rmw_client_t * rmw_create_client( 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); - }); + rcpputils::make_scope_exit( + [&allocator, &type_hash_c_str]() { + allocator->deallocate(type_hash_c_str, allocator->state); + }); client_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), @@ -2178,7 +2296,8 @@ rmw_client_t * rmw_create_client( return nullptr; } - if (z_keyexpr_from_str(&client_data->keyexpr, + if (z_keyexpr_from_str( + &client_data->keyexpr, client_data->entity->topic_info()->topic_keyexpr_.c_str())) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); @@ -2188,16 +2307,16 @@ rmw_client_t * rmw_create_client( std::string liveliness_keyexpr = client_data->entity->liveliness_keyexpr(); z_view_keyexpr_t liveliness_ke; z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); - if(zc_liveliness_declare_token( - &client_data->token, - z_loan(node->context->impl->session), - z_loan(liveliness_ke), - NULL + if (zc_liveliness_declare_token( + &client_data->token, + z_loan(node->context->impl->session), + z_loan(liveliness_ke), + NULL )) { 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; } @@ -2217,37 +2336,42 @@ rmw_client_t * 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); - - rcutils_allocator_t *allocator = &node->context->options.allocator; - - rmw_zenoh_cpp::rmw_client_data_t *client_data = + 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; + + 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_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 @@ -2257,8 +2381,9 @@ rmw_ret_t rmw_destroy_client(rmw_node_t *node, rmw_client_t *client) 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; @@ -2267,49 +2392,52 @@ rmw_ret_t 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) + 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_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 = + 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_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 = + 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)); + ros_request, client_data->request_type_support_impl)); // Init serialized message byte array - char *request_bytes = static_cast( + 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); - }); + 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); @@ -2317,7 +2445,7 @@ rmw_ret_t 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; } @@ -2358,15 +2486,17 @@ rmw_ret_t rmw_send_request( opts.consolidation = z_query_consolidation_latest(); z_owned_bytes_t payload; - z_bytes_serialize_from_buf(&payload, - reinterpret_cast(request_bytes), - data_length); + z_bytes_serialize_from_buf( + &payload, + reinterpret_cast(request_bytes), + data_length); opts.payload = z_move(payload); z_owned_closure_reply_t callback; z_closure(&callback, rmw_zenoh_cpp::client_data_handler, NULL, client_data); - z_get(z_loan(context_impl->session), z_loan(client_data->keyexpr), "", - z_move(callback), &opts); + z_get( + z_loan(context_impl->session), z_loan(client_data->keyexpr), "", + z_move(callback), &opts); return RMW_RET_OK; } @@ -2374,9 +2504,9 @@ rmw_ret_t rmw_send_request( //============================================================================== /// 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) + const rmw_client_t * client, + rmw_service_info_t * request_header, + void * ros_response, bool * taken) { *taken = false; @@ -2384,18 +2514,21 @@ rmw_ret_t rmw_take_response( 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 = + 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); + 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(); @@ -2405,7 +2538,7 @@ rmw_ret_t rmw_take_response( return RMW_RET_OK; } - const z_loaned_sample_t *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; @@ -2423,8 +2556,8 @@ rmw_ret_t rmw_take_response( // 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; @@ -2433,25 +2566,26 @@ rmw_ret_t rmw_take_response( // Fill in the request_header request_header->request_id.sequence_number = - rmw_zenoh_cpp::get_int64_from_attachment(z_sample_attachment(sample), - "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"); + "Failed to get sequence_number from client call attachment"); return RMW_RET_ERROR; } request_header->source_timestamp = rmw_zenoh_cpp::get_int64_from_attachment( - z_sample_attachment(sample), "source_timestamp"); + 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"); + "Failed to get source_timestamp from client call attachment"); return RMW_RET_ERROR; } if (!rmw_zenoh_cpp::get_gid_from_attachment( - z_sample_attachment(sample), - 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; @@ -2471,16 +2605,17 @@ rmw_ret_t 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) + 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 = + rmw_zenoh_cpp::rmw_client_data_t * client_data = static_cast(client->data); RMW_CHECK_ARGUMENT_FOR_NULL(client_data, RMW_RET_INVALID_ARGUMENT); @@ -2492,8 +2627,8 @@ 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) + 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); @@ -2503,14 +2638,15 @@ rmw_client_response_subscription_get_actual_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) + 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)) { @@ -2528,74 +2664,84 @@ rmw_service_t * rmw_create_service( return nullptr; } if (RMW_TOPIC_VALID != validation_result) { - const char *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); + "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 = + 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."); 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 = + 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); + 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); // 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( + 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); + 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); - }); + 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); + 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); - }); + 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_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 = + 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 @@ -2618,57 +2764,64 @@ rmw_service_t * rmw_create_service( // Request type support service_data->request_type_support = static_cast(allocator->allocate( - sizeof(rmw_zenoh_cpp::RequestTypeSupport), allocator->state)); + 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); + }); + 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); - }); + 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)); + 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); + }); + 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); - }); + 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); + 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); - }); + 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 @@ -2679,24 +2832,25 @@ rmw_service_t * rmw_create_service( 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); - }); + rcpputils::make_scope_exit( + [&allocator, &type_hash_c_str]() { + allocator->deallocate(type_hash_c_str, allocator->state); + }); service_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), @@ -2720,7 +2874,8 @@ rmw_service_t * rmw_create_service( rmw_service->service_name); return nullptr; } - if(z_keyexpr_from_str(&service_data->keyexpr, + if (z_keyexpr_from_str( + &service_data->keyexpr, service_data->entity->topic_info()->topic_keyexpr_.c_str())) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); @@ -2728,13 +2883,14 @@ rmw_service_t * rmw_create_service( } z_owned_closure_query_t callback; - z_closure(&callback, rmw_zenoh_cpp::service_data_handler, nullptr, - service_data); + 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(&qable_options); qable_options.complete = true; - if(z_declare_queryable( + if (z_declare_queryable( &service_data->qable, z_loan(context_impl->session), z_loan(service_data->keyexpr), z_move(callback), &qable_options)) { @@ -2748,14 +2904,14 @@ rmw_service_t * rmw_create_service( z_view_keyexpr_t liveliness_ke; z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); if (zc_liveliness_declare_token( - &service_data->token, - z_loan(node->context->impl->session), - z_loan(liveliness_ke), - NULL + &service_data->token, + z_loan(node->context->impl->session), + z_loan(liveliness_ke), + NULL )) { 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; } @@ -2776,47 +2932,53 @@ rmw_service_t * rmw_create_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); - - rcutils_allocator_t *allocator = &node->context->options.allocator; - - rmw_zenoh_cpp::rmw_service_data_t *service_data = + 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; + + 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_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; @@ -2825,9 +2987,9 @@ rmw_ret_t 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) + const rmw_service_t * service, + rmw_service_info_t * request_header, + void * ros_request, bool * taken) { *taken = false; @@ -2836,19 +2998,22 @@ rmw_ret_t 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 = + 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_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(); @@ -2858,7 +3023,7 @@ rmw_ret_t rmw_take_request( return RMW_RET_OK; } - const z_loaned_query_t *loaned_query = query->get_query(); + const z_loaned_query_t * loaned_query = query->get_query(); // DESERIALIZE MESSAGE // ======================================================== @@ -2874,8 +3039,8 @@ rmw_ret_t rmw_take_request( // 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; @@ -2884,13 +3049,13 @@ rmw_ret_t rmw_take_request( // Fill in the request header. // Get the sequence_number out of the attachment - const z_loaned_bytes_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"); if (request_header->request_id.sequence_number < 0) { RMW_SET_ERROR_MSG( - "Failed to get sequence_number from client call attachment"); + "Failed to get sequence_number from client call attachment"); return RMW_RET_ERROR; } @@ -2898,12 +3063,12 @@ rmw_ret_t rmw_take_request( 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"); + "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; @@ -2915,8 +3080,9 @@ rmw_ret_t rmw_take_request( // 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))) + 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; @@ -2931,24 +3097,26 @@ rmw_ret_t 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) + 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 = + rmw_zenoh_cpp::rmw_service_data_t * service_data = static_cast(service->data); // Create the queryable payload @@ -2960,23 +3128,24 @@ rmw_ret_t rmw_send_response( 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)); + ros_response, service_data->response_type_support_impl)); // Init serialized message byte array - char *response_bytes = static_cast( + 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); - }); + 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); @@ -2984,20 +3153,21 @@ rmw_ret_t 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_loaned_query_t *loaned_query = query->get_query(); + 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_t attachment; - if (!create_map_and_set_sequence_num(&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 @@ -3007,9 +3177,10 @@ rmw_ret_t rmw_send_response( z_owned_bytes_t payload; 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); + &payload, reinterpret_cast(response_bytes), data_length); + z_query_reply( + loaned_query, z_loan(service_data->keyexpr), z_move(payload), + &options); z_drop(z_move(payload)); @@ -3020,16 +3191,17 @@ rmw_ret_t 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) + 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 = + rmw_zenoh_cpp::rmw_service_data_t * service_data = static_cast(service->data); RMW_CHECK_ARGUMENT_FOR_NULL(service_data, RMW_RET_INVALID_ARGUMENT); @@ -3041,8 +3213,8 @@ 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) + 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); @@ -3050,43 +3222,47 @@ rmw_service_response_publisher_get_actual_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) +rmw_guard_condition_t * rmw_create_guard_condition(rmw_context_t * context) { - rcutils_allocator_t *allocator = &context->options.allocator; + 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); + 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); - }); + 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); + 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); - }); + 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); - }); + 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(); @@ -3097,11 +3273,11 @@ rmw_guard_condition_t * rmw_create_guard_condition(rmw_context_t *context) /// 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_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) @@ -3116,13 +3292,14 @@ rmw_ret_t 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(); @@ -3133,44 +3310,49 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t *guard_condition) //============================================================================== /// 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, + 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); - }); + 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( + 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_t(), rmw_wait_set_data); - }); + }); static_cast(wait_set->data)->context = context; @@ -3184,19 +3366,20 @@ rmw_wait_set_t * rmw_create_wait_set( //============================================================================== /// 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); - 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); @@ -3213,11 +3396,11 @@ bool check_and_attach_condition( 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) + 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 = + rmw_zenoh_cpp::GuardCondition * gc = static_cast( guard_conditions->guard_conditions[i]); if (gc == nullptr) { @@ -3236,9 +3419,9 @@ bool check_and_attach_condition( 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; } @@ -3249,7 +3432,7 @@ bool check_and_attach_condition( } if (event_data->queue_has_data_and_attach_condition_if_not( - zenoh_event_type, wait_set_data)) + zenoh_event_type, wait_set_data)) { return true; } @@ -3277,7 +3460,7 @@ bool check_and_attach_condition( continue; } if (serv_data->queue_has_data_and_attach_condition_if_not( - wait_set_data)) + wait_set_data)) { return true; } @@ -3286,13 +3469,13 @@ bool 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 = + 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)) + wait_set_data)) { return true; } @@ -3306,22 +3489,24 @@ bool check_and_attach_condition( //============================================================================== /// 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_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); + 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_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. @@ -3342,8 +3527,9 @@ rmw_ret_t rmw_wait( // 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); + 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); @@ -3352,13 +3538,14 @@ rmw_ret_t rmw_wait( // 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)), + lock, + std::chrono::nanoseconds( + wait_timeout->nsec + + RCUTILS_S_TO_NS(wait_timeout->sec)), [wait_set_data]() {return wait_set_data->triggered;}); } } @@ -3377,7 +3564,7 @@ rmw_ret_t rmw_wait( // 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 = + rmw_zenoh_cpp::GuardCondition * gc = static_cast( guard_conditions->guard_conditions[i]); if (gc == nullptr) { @@ -3409,7 +3596,7 @@ rmw_ret_t rmw_wait( } if (event_data->detach_condition_and_event_queue_is_empty( - zenoh_event_type)) + zenoh_event_type)) { // Setting to nullptr lets rcl know that this subscription is not ready events->events[i] = nullptr; @@ -3455,7 +3642,7 @@ rmw_ret_t rmw_wait( if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { - rmw_zenoh_cpp::rmw_client_data_t *client_data = + rmw_zenoh_cpp::rmw_client_data_t * client_data = static_cast(clients->clients[i]); if (client_data == nullptr) { continue; @@ -3476,9 +3663,9 @@ rmw_ret_t 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) + 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); @@ -3486,18 +3673,18 @@ rmw_ret_t rmw_get_node_names( 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) + 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); @@ -3506,23 +3693,24 @@ rmw_ret_t 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) + 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 = @@ -3531,10 +3719,11 @@ rmw_ret_t rmw_count_publishers( return ret; } if (RMW_TOPIC_VALID != validation_result) { - const char *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); + 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); @@ -3545,13 +3734,14 @@ rmw_ret_t 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) + 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 = @@ -3560,28 +3750,31 @@ rmw_ret_t rmw_count_subscribers( return ret; } if (RMW_TOPIC_VALID != validation_result) { - const char *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); + 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) + 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 = @@ -3590,10 +3783,11 @@ rmw_ret_t rmw_count_clients( return ret; } if (RMW_TOPIC_VALID != validation_result) { - const char *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); + 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); @@ -3604,13 +3798,14 @@ rmw_ret_t 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) + 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 = @@ -3619,10 +3814,11 @@ rmw_ret_t rmw_count_services( return ret; } if (RMW_TOPIC_VALID != validation_result) { - const char *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); + 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); @@ -3633,13 +3829,13 @@ rmw_ret_t 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) + 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 = + rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); @@ -3651,12 +3847,12 @@ rmw_ret_t rmw_get_gid_for_publisher( //============================================================================== /// 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 = + rmw_zenoh_cpp::rmw_client_data_t * client_data = static_cast(client->data); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; @@ -3668,17 +3864,19 @@ rmw_ret_t 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) + 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; @@ -3689,24 +3887,25 @@ rmw_ret_t rmw_compare_gids_equal( //============================================================================== /// 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) + 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 = + 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; } @@ -3716,13 +3915,13 @@ rmw_ret_t 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); } //============================================================================== @@ -3755,12 +3954,12 @@ rmw_ret_t 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_subscription_t * subscription, rmw_event_callback_t callback, - const void *user_data) + const void * user_data) { RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_subscription_data_t *sub_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); @@ -3770,12 +3969,12 @@ rmw_subscription_set_on_new_message_callback( //============================================================================== /// 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_service_t * service, rmw_event_callback_t callback, - const void *user_data) + const void * user_data) { RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_service_data_t *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); @@ -3785,12 +3984,12 @@ rmw_ret_t rmw_service_set_on_new_request_callback( //============================================================================== /// 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_client_t * client, rmw_event_callback_t callback, - const void *user_data) + const void * user_data) { RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - rmw_zenoh_cpp::rmw_client_data_t *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); From 1630e11a06fc82a92ad7415220d6b532a5d024a4 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 23:36:24 +0800 Subject: [PATCH 41/77] test: check uncrustify version --- .github/workflows/style.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index 1581e8c7..13af605e 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -18,6 +18,8 @@ jobs: timeout-minutes: 30 steps: - uses: actions/checkout@v4 + - name: check uncrustify version + run: apt-cache madison ros-iron-ament-uncrustify - name: uncrustify run: /ros_entrypoint.sh ament_uncrustify rmw_zenoh_cpp/ --exclude rmw_zenoh_cpp/src/detail/ordered_hash.hpp rmw_zenoh_cpp/src/detail/ordered_map.hpp - name: cpplint From c0a7533db47a7ac0d29f654ac315dd7247971d16 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 5 Sep 2024 23:40:25 +0800 Subject: [PATCH 42/77] fixup! test: check uncrustify version --- .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 13af605e..05f50511 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -19,7 +19,7 @@ jobs: steps: - uses: actions/checkout@v4 - name: check uncrustify version - run: apt-cache madison ros-iron-ament-uncrustify + run: sudo apt update -y && sudo apt install -y ros-iron-ament-uncrustify - name: uncrustify run: /ros_entrypoint.sh ament_uncrustify rmw_zenoh_cpp/ --exclude rmw_zenoh_cpp/src/detail/ordered_hash.hpp rmw_zenoh_cpp/src/detail/ordered_map.hpp - name: cpplint From 798b006192e69062a9c215ef4fe52622daa55aac Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Fri, 6 Sep 2024 00:25:14 +0800 Subject: [PATCH 43/77] chore: make iron ament_uncrustify happy --- .github/workflows/style.yaml | 2 -- rmw_zenoh_cpp/src/detail/attachment_helpers.cpp | 6 +++--- rmw_zenoh_cpp/src/detail/attachment_helpers.hpp | 6 +++--- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 4 ++-- rmw_zenoh_cpp/src/detail/logging_macros.hpp | 10 +++++----- rmw_zenoh_cpp/src/detail/qos.hpp | 2 +- rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp | 8 ++++---- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 8 ++++---- 8 files changed, 22 insertions(+), 24 deletions(-) diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index 05f50511..1581e8c7 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -18,8 +18,6 @@ jobs: timeout-minutes: 30 steps: - uses: actions/checkout@v4 - - name: check uncrustify version - run: sudo apt update -y && sudo apt install -y ros-iron-ament-uncrustify - name: uncrustify run: /ros_entrypoint.sh ament_uncrustify rmw_zenoh_cpp/ --exclude rmw_zenoh_cpp/src/detail/ordered_hash.hpp rmw_zenoh_cpp/src/detail/ordered_map.hpp - name: cpplint diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 8ce5348b..fe100f21 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -61,7 +61,7 @@ z_result_t attachement_data_t::serialize_to_zbytes(z_owned_bytes_t * attachment) } bool get_attachment( - const z_loaned_bytes_t *const attachment, + const z_loaned_bytes_t * const attachment, const std::string & key, z_owned_bytes_t * val) { if (z_bytes_is_empty(attachment)) { @@ -104,7 +104,7 @@ bool get_attachment( } bool get_gid_from_attachment( - const z_loaned_bytes_t *const attachment, + const z_loaned_bytes_t * const attachment, uint8_t gid[RMW_GID_STORAGE_SIZE]) { if (z_bytes_is_empty(attachment)) { @@ -131,7 +131,7 @@ bool get_gid_from_attachment( } int64_t get_int64_from_attachment( - const z_loaned_bytes_t *const attachment, + const z_loaned_bytes_t * const attachment, const std::string & name) { // A valid request must have had an attachment diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp index 2d663706..d01d3e8f 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -54,15 +54,15 @@ class attachement_context_t final }; bool get_attachment( - const z_loaned_bytes_t *const 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, + 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 z_loaned_bytes_t * const attachment, const std::string & name); } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index e5928237..ceccbe82 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -696,8 +696,8 @@ rmw_ret_t GraphCache::get_node_names( rcutils_ret_t ret = rcutils_string_array_fini(enclaves); if (ret != RCUTILS_RET_OK) { RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "failed to cleanup during error handling: %s", rcutils_get_error_string().str); + "rmw_zenoh_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string().str); } }; diff --git a/rmw_zenoh_cpp/src/detail/logging_macros.hpp b/rmw_zenoh_cpp/src/detail/logging_macros.hpp index 44a4bc34..81201856 100644 --- a/rmw_zenoh_cpp/src/detail/logging_macros.hpp +++ b/rmw_zenoh_cpp/src/detail/logging_macros.hpp @@ -24,14 +24,14 @@ // invoke GraphCache::parse_put() and GraphCache::parse_del() functions. // See https://github.com/ros2/rmw_zenoh/issues/182 for more details. #define RMW_ZENOH_LOG_DEBUG_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_DEBUG, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_DEBUG, __func__, __FILE__, __LINE__, __VA_ARGS__);} #define RMW_ZENOH_LOG_ERROR_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_ERROR, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_ERROR, __func__, __FILE__, __LINE__, __VA_ARGS__);} #define RMW_ZENOH_LOG_FATAL_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_FATAL, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_FATAL, __func__, __FILE__, __LINE__, __VA_ARGS__);} #define RMW_ZENOH_LOG_INFO_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_INFO, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_INFO, __func__, __FILE__, __LINE__, __VA_ARGS__);} #define RMW_ZENOH_LOG_WARN_NAMED(...) {rmw_zenoh_cpp::Logger::get().log_named( \ - RCUTILS_LOG_SEVERITY_WARN, __func__, __FILE__, __LINE__, __VA_ARGS__);} + RCUTILS_LOG_SEVERITY_WARN, __func__, __FILE__, __LINE__, __VA_ARGS__);} #endif // DETAIL__LOGGING_MACROS_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/qos.hpp b/rmw_zenoh_cpp/src/detail/qos.hpp index 6b8e9230..eee74c05 100644 --- a/rmw_zenoh_cpp/src/detail/qos.hpp +++ b/rmw_zenoh_cpp/src/detail/qos.hpp @@ -25,7 +25,7 @@ namespace rmw_zenoh_cpp { //============================================================================== /// Signature matching rmw_get_publishers_info_by_topic and rmw_get_subscriptions_info_by_topic -using GetEndpointInfoByTopicFunction = std::function(ctx)))++; }; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index a5aff757..96741c62 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -3392,10 +3392,10 @@ rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t * wait_set) 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, + 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) { From 7d7a296159514e185784d950c92ec14f89f86526 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Fri, 6 Sep 2024 00:39:14 +0800 Subject: [PATCH 44/77] test --- .github/workflows/style.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index 1581e8c7..79ffe144 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -18,6 +18,8 @@ jobs: timeout-minutes: 30 steps: - uses: actions/checkout@v4 + - name: check version + run: /ros_entrypoint.sh lsb_release -a - name: uncrustify run: /ros_entrypoint.sh ament_uncrustify rmw_zenoh_cpp/ --exclude rmw_zenoh_cpp/src/detail/ordered_hash.hpp rmw_zenoh_cpp/src/detail/ordered_map.hpp - name: cpplint From 30aae8082407794f98b7158a5bc48e7c66eee633 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Fri, 6 Sep 2024 00:42:05 +0800 Subject: [PATCH 45/77] Revert "test" This reverts commit 7d7a296159514e185784d950c92ec14f89f86526. --- .github/workflows/style.yaml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index 79ffe144..1581e8c7 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -18,8 +18,6 @@ jobs: timeout-minutes: 30 steps: - uses: actions/checkout@v4 - - name: check version - run: /ros_entrypoint.sh lsb_release -a - name: uncrustify run: /ros_entrypoint.sh ament_uncrustify rmw_zenoh_cpp/ --exclude rmw_zenoh_cpp/src/detail/ordered_hash.hpp rmw_zenoh_cpp/src/detail/ordered_map.hpp - name: cpplint From 4f369c5b1cf3a9ce69d2bd4e3bd53846dc2ed2fb Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Mon, 9 Sep 2024 15:36:21 +0800 Subject: [PATCH 46/77] build: bump up the zenoh-c commit --- 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 ef90d646..33e7e730 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -26,7 +26,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - 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 + VCS_VERSION 8d5b11ce114a4eb591f15891878badeb53e56c48 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" From 39f10d506ddad22890c0b526bddd6fc50e1b8a30 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 10 Sep 2024 12:29:28 +0800 Subject: [PATCH 47/77] chore(style): align rmw_zenoh.cpp --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 1711 +++++++++++++++---------------- 1 file changed, 824 insertions(+), 887 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 96741c62..eb991a98 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include - #include #include @@ -29,8 +28,8 @@ #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" @@ -39,11 +38,8 @@ #include "detail/rmw_data_types.hpp" #include "detail/serialization_format.hpp" #include "detail/type_support_common.hpp" - #include "rcpputils/scope_exit.hpp" - #include "rcutils/strdup.h" - #include "rmw/dynamic_message_type_support.h" #include "rmw/error_handling.h" #include "rmw/features.h" @@ -64,8 +60,7 @@ const rosidl_message_type_support_t * find_message_type_support( 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(); @@ -83,13 +78,11 @@ 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 * 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 * 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(); @@ -113,21 +106,24 @@ find_service_type_support(const rosidl_service_type_support_t * type_supports) } // namespace -extern "C" { +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) +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; } @@ -151,21 +147,26 @@ 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, +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, + context, + context->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG( - context->impl, "expected initialized context", + context->impl, + "expected initialized context", return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG( context->impl->enclave, - "expected initialized enclave", return nullptr); + "expected initialized enclave", + return nullptr); if (context->impl->is_shutdown) { RCUTILS_SET_ERROR_MSG("context has been shutdown"); return nullptr; @@ -177,8 +178,7 @@ rmw_node_t * 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; } @@ -189,25 +189,29 @@ rmw_node_t * 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; - rmw_node_t * node = static_cast( - allocator->zero_allocate(1, sizeof(rmw_node_t), allocator->state)); + 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", + 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); + 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); @@ -220,17 +224,16 @@ rmw_node_t * rmw_create_node( return nullptr); auto free_namespace = rcpputils::make_scope_exit( [node, allocator]() { - allocator->deallocate( - const_cast(node->namespace_), - allocator->state); + allocator->deallocate(const_cast(node->namespace_), allocator->state); }); // 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); + 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); @@ -242,12 +245,10 @@ rmw_node_t * rmw_create_node( 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); + 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. + // 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)), @@ -259,32 +260,27 @@ rmw_node_t * rmw_create_node( if (node_data->entity == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", - "Unable to generate keyexpr for liveliness token for the node."); + "Unable to generate keyexpr for liveliness token for the node %s.", + name); return nullptr; } std::string liveliness_keyexpr = node_data->entity->liveliness_keyexpr(); z_view_keyexpr_t keyexpr; - if (z_view_keyexpr_from_str(&keyexpr, liveliness_keyexpr.c_str())) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", "Unable to generate keyexpr from the entity string."); - return nullptr; - } - + z_view_keyexpr_from_str(&keyexpr, liveliness_keyexpr.c_str()); + auto free_token = rcpputils::make_scope_exit( + [node_data]() { + z_drop(z_move(node_data->token)); + }); if (zc_liveliness_declare_token( - &node_data->token, - z_loan(context->impl->session), - z_loan(keyexpr), NULL)) + &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."); + "Unable to create liveliness token for the node."); return nullptr; } - auto free_token = rcpputils::make_scope_exit( - [node_data]() {z_drop(z_move(node_data->token));}); - node->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; node->context = context; node->data = node_data; @@ -299,23 +295,24 @@ rmw_node_t * 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, + node, + node->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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. + // 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) { @@ -338,7 +335,8 @@ 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, + node, + node->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); @@ -350,7 +348,8 @@ 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( +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) @@ -365,7 +364,8 @@ rmw_ret_t 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"); @@ -374,13 +374,13 @@ rmw_fini_publisher_allocation(rmw_publisher_allocation_t * allocation) namespace { -void generate_random_gid(uint8_t gid[RMW_GID_STORAGE_SIZE]) +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); @@ -390,14 +390,18 @@ void generate_random_gid(uint8_t gid[RMW_GID_STORAGE_SIZE]) //============================================================================== /// 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, +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, + node, + node->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); @@ -409,14 +413,12 @@ rmw_publisher_t * 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; } @@ -426,8 +428,7 @@ rmw_publisher_t * rmw_create_publisher( RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED) { RMW_SET_ERROR_MSG( - "Strict requirement on unique network flow endpoints for " - "publishers not supported"); + "Strict requirement on unique network flow endpoints for publishers not supported"); return nullptr; } const rmw_zenoh_cpp::rmw_node_data_t * node_data = @@ -435,54 +436,53 @@ rmw_publisher_t * rmw_create_publisher( 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", + node->context, + "expected initialized context", return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG( - node->context, "expected initialized context", + 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( - 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", + 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); + "expected initialized enclave", + return nullptr); 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)); + 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( + 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)); + 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( + auto free_publisher_data = rcpputils::make_scope_exit( [publisher_data, allocator]() { allocator->deallocate(publisher_data, allocator->state); }); @@ -493,8 +493,7 @@ rmw_publisher_t * rmw_create_publisher( 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); + publisher_data->~rmw_publisher_data_t(), rmw_zenoh_cpp::rmw_publisher_data_t); }); generate_random_gid(publisher_data->pub_gid); @@ -502,8 +501,7 @@ rmw_publisher_t * rmw_create_publisher( // 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; } @@ -511,27 +509,24 @@ rmw_publisher_t * 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)); + 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( + 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, + publisher_data->type_support, + return nullptr, rmw_zenoh_cpp::MessageTypeSupport, callbacks); - auto destruct_msg_type_support = - rcpputils::make_scope_exit( + auto destruct_msg_type_support = rcpputils::make_scope_exit( [publisher_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( publisher_data->type_support->~MessageTypeSupport(), @@ -540,34 +535,32 @@ rmw_publisher_t * rmw_create_publisher( 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( + "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); + 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; 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( + auto free_type_hash_c_str = rcpputils::make_scope_exit( [&allocator, &type_hash_c_str]() { allocator->deallocate(type_hash_c_str, allocator->state); }); @@ -602,9 +595,7 @@ rmw_publisher_t * rmw_create_publisher( z_view_keyexpr_from_str(&pub_ke, topic_keyexpr.c_str()); // Create a Publication Cache if durability is transient_local. - if (publisher_data->adapted_qos_profile.durability == - RMW_QOS_POLICY_DURABILITY_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(&pub_cache_opts); pub_cache_opts.history = publisher_data->adapted_qos_profile.depth; @@ -622,19 +613,14 @@ rmw_publisher_t * rmw_create_publisher( ze_owned_publication_cache_t pub_cache; if (ze_declare_publication_cache( - &pub_cache, - z_loan(context_impl->session), - z_loan(pub_ke), - &pub_cache_opts - )) + &pub_cache, z_loan(context_impl->session), z_loan(pub_ke), &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( + 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())); @@ -645,18 +631,17 @@ rmw_publisher_t * rmw_create_publisher( 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) - { - opts.congestion_control = Z_CONGESTION_CONTROL_BLOCK; + if (publisher_data->adapted_qos_profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { + opts.reliability = Z_RELIABILITY_RELIABLE; + if (publisher_data->adapted_qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) { + opts.congestion_control = Z_CONGESTION_CONTROL_BLOCK; + } + } else { + opts.reliability = Z_RELIABILITY_BEST_EFFORT; } - // TODO(clalancette): What happens if the key name is a valid but empty - // string? + // 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(pub_ke), &opts) < 0) + &publisher_data->pub, z_loan(context_impl->session), z_loan(pub_ke), &opts)) { RMW_SET_ERROR_MSG("unable to create zenoh publisher"); return nullptr; @@ -669,12 +654,15 @@ rmw_publisher_t * rmw_create_publisher( std::string liveliness_keyexpr = publisher_data->entity->liveliness_keyexpr(); z_view_keyexpr_t liveliness_ke; z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); + auto free_token = rcpputils::make_scope_exit( + [publisher_data]() { + if (publisher_data != nullptr) { + z_drop(z_move(publisher_data->token)); + } + }); if (zc_liveliness_declare_token( - &publisher_data->token, - z_loan(node->context->impl->session), - z_loan(liveliness_ke), - NULL - )) + &publisher_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), + NULL)) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -682,6 +670,7 @@ rmw_publisher_t * rmw_create_publisher( return nullptr; } + free_token.cancel(); undeclare_z_publisher_cache.cancel(); undeclare_z_publisher.cancel(); free_topic_name.cancel(); @@ -697,13 +686,15 @@ rmw_publisher_t * 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) +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, + node, + node->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -716,38 +707,33 @@ rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) 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( +rmw_ret_t +rmw_take_dynamic_message( const rmw_subscription_t * subscription, - rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, bool * taken, + rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, + bool * taken, rmw_subscription_allocation_t * allocation) { static_cast(subscription); @@ -758,9 +744,11 @@ rmw_ret_t rmw_take_dynamic_message( } //============================================================================== -rmw_ret_t rmw_take_dynamic_message_with_info( +rmw_ret_t +rmw_take_dynamic_message_with_info( const rmw_subscription_t * subscription, - rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, bool * taken, + rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, + bool * taken, rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation) { @@ -773,8 +761,10 @@ rmw_ret_t rmw_take_dynamic_message_with_info( } //============================================================================== -rmw_ret_t rmw_serialization_support_init( - const char * serialization_lib_name, rcutils_allocator_t * allocator, +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); @@ -821,8 +811,7 @@ create_map_and_set_sequence_num( 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); + 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", @@ -836,8 +825,10 @@ create_map_and_set_sequence_num( //============================================================================== /// Publish a ROS message. -rmw_ret_t rmw_publish( - const rmw_publisher_t * publisher, const void * ros_message, +rmw_ret_t +rmw_publish( + const rmw_publisher_t * publisher, + const void * ros_message, rmw_publisher_allocation_t * allocation) { static_cast(allocation); @@ -845,80 +836,65 @@ rmw_ret_t rmw_publish( 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, + 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); + 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); + 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; - - // 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( + 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"); + 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); + 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))); + 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."); + "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)); + 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); + msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); } // Object that manages the raw buffer @@ -927,7 +903,9 @@ rmw_ret_t 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; @@ -943,7 +921,9 @@ rmw_ret_t rmw_publish( return RMW_RET_ERROR; } auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() {z_drop(z_move(attachment));}); + [&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 @@ -954,17 +934,15 @@ rmw_ret_t rmw_publish( z_owned_bytes_t payload; - // TODO(yuyuan): SHM 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 { - 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; - } + 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; @@ -972,7 +950,8 @@ rmw_ret_t rmw_publish( //============================================================================== /// Publish a loaned ROS message. -rmw_ret_t rmw_publish_loaned_message( +rmw_ret_t +rmw_publish_loaned_message( const rmw_publisher_t * publisher, void * ros_message, rmw_publisher_allocation_t * allocation) @@ -1003,8 +982,7 @@ rmw_publisher_count_matched_subscriptions( 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( @@ -1013,7 +991,8 @@ rmw_publisher_count_matched_subscriptions( //============================================================================== /// Retrieve the actual qos settings of the publisher. -rmw_ret_t rmw_publisher_get_actual_qos( +rmw_ret_t +rmw_publisher_get_actual_qos( const rmw_publisher_t * publisher, rmw_qos_profile_t * qos) { @@ -1035,7 +1014,8 @@ rmw_ret_t rmw_publisher_get_actual_qos( //============================================================================== /// Publish a ROS message as a byte stream. -rmw_ret_t rmw_publish_serialized_message( +rmw_ret_t +rmw_publish_serialized_message( const rmw_publisher_t * publisher, const rmw_serialized_message_t * serialized_message, rmw_publisher_allocation_t * allocation) @@ -1045,23 +1025,19 @@ rmw_ret_t rmw_publish_serialized_message( 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, + 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", + serialized_message, "serialized message handle is null", return RMW_RET_INVALID_ARGUMENT); - auto publisher_data = - static_cast(publisher->data); + 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"); @@ -1075,8 +1051,11 @@ rmw_ret_t rmw_publish_serialized_message( // 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));}); + auto free_attachment = + rcpputils::make_scope_exit( + [&attachment]() { + z_drop(z_move(attachment)); + }); const size_t data_length = ser.get_serialized_data_length(); @@ -1100,9 +1079,11 @@ rmw_ret_t rmw_publish_serialized_message( //============================================================================== /// Compute the size of a serialized message. -rmw_ret_t rmw_get_serialized_message_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) + const rosidl_runtime_c__Sequence__bound * message_bounds, + size_t * size) { static_cast(type_support); static_cast(message_bounds); @@ -1111,9 +1092,9 @@ rmw_ret_t 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) +/// 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", @@ -1129,46 +1110,42 @@ rmw_ret_t 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( +/// 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( +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); + 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; } @@ -1186,34 +1163,32 @@ rmw_ret_t rmw_serialize( //============================================================================== /// Deserialize a ROS message. -rmw_ret_t rmw_deserialize( +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); + 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( +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) @@ -1228,7 +1203,8 @@ rmw_ret_t 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; @@ -1236,14 +1212,18 @@ rmw_fini_subscription_allocation(rmw_subscription_allocation_t * allocation) //============================================================================== /// 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, +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, + node, + node->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); @@ -1255,8 +1235,7 @@ rmw_subscription_t * 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; @@ -1267,24 +1246,26 @@ rmw_subscription_t * rmw_create_subscription( 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; + // 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", + node->context, + "expected initialized context", return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG( - node->context, "expected initialized context", + 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( - 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", + 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); + "expected initialized enclave", + return nullptr); rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -1296,15 +1277,13 @@ rmw_subscription_t * rmw_create_subscription( rmw_subscription, "failed to allocate memory for the subscription", return nullptr); - auto free_rmw_subscription = - rcpputils::make_scope_exit( + 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)); + 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", @@ -1314,9 +1293,7 @@ rmw_subscription_t * rmw_create_subscription( allocator->deallocate(sub_data, allocator->state); }); - RMW_TRY_PLACEMENT_NEW( - sub_data, sub_data, return nullptr, - rmw_zenoh_cpp::rmw_subscription_data_t); + 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( @@ -1327,8 +1304,7 @@ rmw_subscription_t * rmw_create_subscription( // 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; @@ -1337,11 +1313,9 @@ rmw_subscription_t * 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)); + 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", @@ -1352,9 +1326,10 @@ rmw_subscription_t * rmw_create_subscription( }); RMW_TRY_PLACEMENT_NEW( - sub_data->type_support, sub_data->type_support, - return nullptr, rmw_zenoh_cpp::MessageTypeSupport, - callbacks); + 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( @@ -1364,19 +1339,16 @@ rmw_subscription_t * rmw_create_subscription( 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( + "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); + allocator->deallocate(const_cast(rmw_subscription->topic_name), allocator->state); }); rmw_subscription->options = *subscription_options; @@ -1387,13 +1359,14 @@ rmw_subscription_t * rmw_create_subscription( // the keyexpr. 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( + auto free_type_hash_c_str = rcpputils::make_scope_exit( [&allocator, &type_hash_c_str]() { allocator->deallocate(type_hash_c_str, allocator->state); }); @@ -1453,22 +1426,17 @@ rmw_subscription_t * rmw_create_subscription( // 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; } ze_owned_querying_subscriber_t sub; if (ze_declare_querying_subscriber( - &sub, z_loan(context_impl->session), - z_loan(ke), z_move(callback), - &sub_options)) + &sub, z_loan(context_impl->session), z_loan(ke), z_move(callback), &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } - // Register the querying subscriber with the graph cache to get latest // messages from publishers that were discovered after their first publication. context_impl->graph_cache->set_querying_subscriber_callback( @@ -1481,37 +1449,32 @@ rmw_subscription_t * rmw_create_subscription( const std::string selector = queryable_prefix + "/" + sub_data->entity->topic_info()->topic_keyexpr_; - z_view_keyexpr_t ke; - z_view_keyexpr_from_str(&ke, selector.c_str()); + RMW_ZENOH_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", - "QueryingSubscriberCallback triggered over %s.", - selector.c_str() - ); + "QueryingSubscriberCallback triggered over %s.", selector.c_str()); z_get_options_t opts; z_get_options_default(&opts); opts.timeout_ms = std::numeric_limits::max(); opts.consolidation = z_query_consolidation_latest(); opts.accept_replies = ZC_REPLY_KEYEXPR_ANY; + + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, selector.c_str()); ze_querying_subscriber_get( z_loan(std::get(sub_data->sub)), z_loan(ke), - &opts - ); + &opts); } ); } else { // Create a regular subscriber for all other durability settings. 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; - } z_owned_subscriber_t sub; if (z_declare_subscriber( - &sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), - &sub_options)) + &sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; @@ -1521,16 +1484,13 @@ rmw_subscription_t * rmw_create_subscription( auto undeclare_z_sub = rcpputils::make_scope_exit( [sub_data]() { - 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 || z_undeclare_subscriber(z_move(*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(z_move(*querying_sub))) - { + if (querying_sub == nullptr || ze_undeclare_querying_subscriber(z_move(*querying_sub))) { RMW_SET_ERROR_MSG("failed to undeclare sub"); } } @@ -1540,12 +1500,14 @@ rmw_subscription_t * rmw_create_subscription( std::string liveliness_keyexpr = sub_data->entity->liveliness_keyexpr(); z_view_keyexpr_t liveliness_ke; z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); + auto free_token = rcpputils::make_scope_exit( + [sub_data]() { + if (sub_data != nullptr) { + z_drop(z_move(sub_data->token)); + } + }); if (zc_liveliness_declare_token( - &sub_data->token, - z_loan(context_impl->session), - z_loan(liveliness_ke), - NULL - )) + &sub_data->token, z_loan(context_impl->session), z_loan(liveliness_ke), NULL)) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -1555,6 +1517,7 @@ 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(); @@ -1567,16 +1530,15 @@ rmw_subscription_t * 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, + node, + node->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -1589,19 +1551,15 @@ rmw_ret_t rmw_destroy_subscription( 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(z_move(*sub))) { RMW_SET_ERROR_MSG("failed to undeclare sub"); @@ -1610,22 +1568,16 @@ rmw_ret_t rmw_destroy_subscription( } else { ze_owned_querying_subscriber_t * querying_sub = std::get_if(&sub_data->sub); - if (querying_sub == nullptr || - ze_undeclare_querying_subscriber(z_move(*querying_sub))) - { + if (querying_sub == nullptr || ze_undeclare_querying_subscriber(z_move(*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,8 +1585,10 @@ rmw_ret_t rmw_destroy_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); @@ -1649,8 +1603,7 @@ rmw_ret_t rmw_subscription_count_matched_publishers( 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( @@ -1672,8 +1625,7 @@ rmw_subscription_get_actual_qos( 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,7 +1634,8 @@ rmw_subscription_get_actual_qos( //============================================================================== /// Set the content filter options for the subscription. -rmw_ret_t rmw_subscription_set_content_filter( +rmw_ret_t +rmw_subscription_set_content_filter( rmw_subscription_t * subscription, const rmw_subscription_content_filter_options_t * options) { @@ -1693,8 +1646,10 @@ rmw_ret_t 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_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); @@ -1705,15 +1660,16 @@ rmw_ret_t rmw_subscription_get_content_filter( namespace { -rmw_ret_t __rmw_take_one( +rmw_ret_t +__rmw_take_one( rmw_zenoh_cpp::rmw_subscription_data_t * sub_data, - void * ros_message, rmw_message_info_t * message_info, + 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; @@ -1729,7 +1685,9 @@ rmw_ret_t __rmw_take_one( // 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; @@ -1741,11 +1699,8 @@ rmw_ret_t __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; } @@ -1757,9 +1712,12 @@ rmw_ret_t __rmw_take_one( //============================================================================== /// 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); @@ -1770,14 +1728,12 @@ rmw_ret_t rmw_take( 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, + 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); @@ -1785,9 +1741,11 @@ rmw_ret_t rmw_take( //============================================================================== /// Take an incoming ROS message with its metadata. -rmw_ret_t rmw_take_with_info( +rmw_ret_t +rmw_take_with_info( const rmw_subscription_t * subscription, - void * ros_message, bool * taken, + void * ros_message, + bool * taken, rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation) { @@ -1801,14 +1759,12 @@ rmw_ret_t rmw_take_with_info( 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, + 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); @@ -1816,7 +1772,8 @@ rmw_ret_t rmw_take_with_info( //============================================================================== /// Take multiple incoming ROS messages with their metadata. -rmw_ret_t rmw_take_sequence( +rmw_ret_t +rmw_take_sequence( const rmw_subscription_t * subscription, size_t count, rmw_message_sequence_t * message_sequence, @@ -1834,8 +1791,7 @@ rmw_ret_t rmw_take_sequence( 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, + subscription->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); if (0u == count) { @@ -1855,15 +1811,14 @@ rmw_ret_t 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) { @@ -1879,17 +1834,15 @@ rmw_ret_t rmw_take_sequence( 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; } @@ -1905,10 +1858,12 @@ rmw_ret_t rmw_take_sequence( //============================================================================== namespace { -rmw_ret_t __rmw_take_serialized( +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) + 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); @@ -1918,14 +1873,12 @@ rmw_ret_t __rmw_take_serialized( 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, + 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) { @@ -1934,11 +1887,9 @@ rmw_ret_t __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; } @@ -1963,11 +1914,8 @@ rmw_ret_t __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; } @@ -1986,31 +1934,31 @@ rmw_take_serialized_message( { 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( +rmw_ret_t +rmw_take_serialized_message_with_info( const rmw_subscription_t * subscription, - rmw_serialized_message_t * serialized_message, bool * taken, + 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( +rmw_ret_t +rmw_take_loaned_message( const rmw_subscription_t * subscription, - void ** loaned_message, bool * taken, + void ** loaned_message, + bool * taken, rmw_subscription_allocation_t * allocation) { static_cast(subscription); @@ -2025,7 +1973,8 @@ rmw_ret_t rmw_take_loaned_message( rmw_ret_t rmw_take_loaned_message_with_info( const rmw_subscription_t * subscription, - void ** loaned_message, bool * taken, + void ** loaned_message, + bool * taken, rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation) { @@ -2039,8 +1988,10 @@ 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); @@ -2048,15 +1999,19 @@ rmw_ret_t rmw_return_loaned_message_from_subscription( } //============================================================================== -/// 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, + node, + node->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); @@ -2069,27 +2024,30 @@ rmw_client_t * rmw_create_client( RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_FOR_NULL_WITH_MSG( - node->context, "expected initialized context", + node->context, + "expected initialized context", return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG( - node->context, "expected initialized context", + 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( - 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", + 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); + "expected initialized enclave", + return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); 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; } @@ -2098,47 +2056,43 @@ rmw_client_t * rmw_create_client( // 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 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, + "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); + 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( @@ -2158,15 +2112,13 @@ rmw_client_t * rmw_create_client( } // 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); auto response_members = static_cast( @@ -2179,25 +2131,24 @@ rmw_client_t * 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); + "Failed to allocate rmw_zenoh_cpp::RequestTypeSupport", + return nullptr); auto free_request_type_support = rcpputils::make_scope_exit( - [client_data, - allocator]() { + [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, + client_data->request_type_support, + return nullptr, rmw_zenoh_cpp::RequestTypeSupport, service_members); - auto destruct_request_type_support = - rcpputils::make_scope_exit( + auto destruct_request_type_support = rcpputils::make_scope_exit( [client_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( client_data->request_type_support->~RequestTypeSupport(), @@ -2205,25 +2156,24 @@ rmw_client_t * rmw_create_client( }); // 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); + "Failed to allocate rmw_zenoh_cpp::ResponseTypeSupport", + return nullptr); auto free_response_type_support = rcpputils::make_scope_exit( - [client_data, - allocator]() { + [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, + client_data->response_type_support, + return nullptr, rmw_zenoh_cpp::ResponseTypeSupport, service_members); - auto destruct_response_type_support = - rcpputils::make_scope_exit( + auto destruct_response_type_support = rcpputils::make_scope_exit( [client_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( client_data->response_type_support->~ResponseTypeSupport(), @@ -2235,25 +2185,24 @@ rmw_client_t * rmw_create_client( 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( + "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); + 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. + // 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", + "rmw_zenoh_cpp", + "Unexpected type %s for client %s. Report this bug", service_type.c_str(), rmw_client->service_name); return nullptr; } @@ -2262,13 +2211,14 @@ rmw_client_t * rmw_create_client( // the keyexpr. 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( + auto free_type_hash_c_str = rcpputils::make_scope_exit( [&allocator, &type_hash_c_str]() { allocator->deallocate(type_hash_c_str, allocator->state); }); @@ -2296,9 +2246,12 @@ rmw_client_t * rmw_create_client( return nullptr; } + auto free_ros_keyexpr = rcpputils::make_scope_exit( + [client_data]() { + z_keyexpr_drop(z_move(client_data->keyexpr)); + }); if (z_keyexpr_from_str( - &client_data->keyexpr, - client_data->entity->topic_info()->topic_keyexpr_.c_str())) + &client_data->keyexpr, client_data->entity->topic_info()->topic_keyexpr_.c_str())) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; @@ -2307,12 +2260,14 @@ rmw_client_t * rmw_create_client( std::string liveliness_keyexpr = client_data->entity->liveliness_keyexpr(); z_view_keyexpr_t liveliness_ke; z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); + auto free_token = rcpputils::make_scope_exit( + [client_data]() { + if (client_data != nullptr) { + z_drop(z_move(client_data->token)); + } + }); if (zc_liveliness_declare_token( - &client_data->token, - z_loan(node->context->impl->session), - z_loan(liveliness_ke), - NULL - )) + &client_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL)) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -2322,6 +2277,7 @@ rmw_client_t * rmw_create_client( rmw_client->data = client_data; + free_token.cancel(); free_rmw_client.cancel(); free_client_data.cancel(); destruct_request_type_support.cancel(); @@ -2330,24 +2286,28 @@ rmw_client_t * rmw_create_client( free_response_type_support.cancel(); destruct_client_data.cancel(); free_service_name.cancel(); + free_ros_keyexpr.cancel(); return rmw_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, + 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, + client, + client->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); @@ -2365,25 +2325,22 @@ rmw_ret_t rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) zc_liveliness_undeclare_token(z_move(client_data->token)); RMW_TRY_DESTRUCTOR( - client_data->request_type_support->~RequestTypeSupport(), - rmw_zenoh_cpp::RequestTypeSupport, ); + 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, ); + 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; @@ -2391,8 +2348,10 @@ rmw_ret_t 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, +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); @@ -2400,7 +2359,8 @@ rmw_ret_t rmw_send_request( 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, + client, + client->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); @@ -2415,26 +2375,26 @@ rmw_ret_t rmw_send_request( 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); - size_t max_data_length = - (client_data->request_type_support->get_estimated_serialized_size( + 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( + auto free_request_bytes = rcpputils::make_scope_exit( [request_bytes, allocator]() { allocator->deallocate(request_bytes, allocator->state); }); @@ -2445,7 +2405,9 @@ rmw_ret_t 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; } @@ -2464,7 +2426,9 @@ rmw_ret_t rmw_send_request( return RMW_RET_ERROR; } auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() {z_drop(z_move(attachment));}); + [&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. @@ -2473,40 +2437,40 @@ rmw_ret_t rmw_send_request( opts.attachment = z_move(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(); z_owned_bytes_t payload; z_bytes_serialize_from_buf( - &payload, - reinterpret_cast(request_bytes), - data_length); + &payload, reinterpret_cast(request_bytes), data_length); opts.payload = z_move(payload); z_owned_closure_reply_t callback; z_closure(&callback, rmw_zenoh_cpp::client_data_handler, NULL, client_data); z_get( - z_loan(context_impl->session), z_loan(client_data->keyexpr), "", - z_move(callback), &opts); + 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( +rmw_ret_t +rmw_take_response( const rmw_client_t * client, rmw_service_info_t * request_header, - void * ros_response, bool * taken) + void * ros_response, + bool * taken) { *taken = false; @@ -2515,26 +2479,21 @@ rmw_ret_t rmw_take_response( 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, + 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); + 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); + client->data, "Unable to retrieve client_data from client.", RMW_RET_INVALID_ARGUMENT); - std::unique_ptr latest_reply = - client_data->pop_next_reply(); + 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; } @@ -2549,14 +2508,14 @@ rmw_ret_t rmw_take_response( // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast( - const_cast(z_slice_data(z_loan(payload)))), + 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, + deser.get_cdr(), + ros_response, client_data->response_type_support_impl)) { RMW_SET_ERROR_MSG("could not deserialize ROS response"); @@ -2566,20 +2525,16 @@ rmw_ret_t rmw_take_response( // Fill in the request_header request_header->request_id.sequence_number = - rmw_zenoh_cpp::get_int64_from_attachment( - z_sample_attachment(sample), - "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( - z_sample_attachment(sample), "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; } @@ -2610,7 +2565,8 @@ rmw_client_request_publisher_get_actual_qos( { RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, client->implementation_identifier, + 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); @@ -2635,16 +2591,19 @@ rmw_client_response_subscription_get_actual_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, + node, + node->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); @@ -2656,18 +2615,14 @@ rmw_service_t * 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; } } @@ -2675,48 +2630,52 @@ rmw_service_t * rmw_create_service( 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", + node->context, + "expected initialized context", return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG( - node->context, "expected initialized context", + 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( - 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", + 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); + "expected initialized enclave", + return nullptr); // SERVICE DATA ============================================================== 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, + "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)); + 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( + auto free_service_data = rcpputils::make_scope_exit( [service_data, allocator]() { allocator->deallocate(service_data, allocator->state); }); @@ -2741,15 +2700,13 @@ rmw_service_t * rmw_create_service( } // 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); auto response_members = static_cast( @@ -2762,22 +2719,22 @@ rmw_service_t * 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); + "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, + service_data->request_type_support, + return nullptr, rmw_zenoh_cpp::RequestTypeSupport, service_members); - auto destruct_request_type_support = - rcpputils::make_scope_exit( + auto destruct_request_type_support = rcpputils::make_scope_exit( [service_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( service_data->request_type_support->~RequestTypeSupport(), @@ -2785,23 +2742,22 @@ rmw_service_t * rmw_create_service( }); // 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); + "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]() { + [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, + service_data->response_type_support, + return nullptr, rmw_zenoh_cpp::ResponseTypeSupport, service_members); - auto destruct_response_type_support = - rcpputils::make_scope_exit( + auto destruct_response_type_support = rcpputils::make_scope_exit( [service_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( service_data->response_type_support->~ResponseTypeSupport(), @@ -2815,24 +2771,22 @@ rmw_service_t * rmw_create_service( rmw_service->service_name, "failed to allocate service name", return nullptr); - auto free_service_name = - rcpputils::make_scope_exit( + auto free_service_name = rcpputils::make_scope_exit( [rmw_service, allocator]() { - allocator->deallocate( - const_cast(rmw_service->service_name), - allocator->state); + 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. + // 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", + "rmw_zenoh_cpp", + "Unexpected type %s for service %s. Report this bug", service_type.c_str(), rmw_service->service_name); return nullptr; } @@ -2841,13 +2795,14 @@ rmw_service_t * rmw_create_service( // the keyexpr. 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( + auto free_type_hash_c_str = rcpputils::make_scope_exit( [&allocator, &type_hash_c_str]() { allocator->deallocate(type_hash_c_str, allocator->state); }); @@ -2875,43 +2830,45 @@ rmw_service_t * rmw_create_service( return nullptr; } if (z_keyexpr_from_str( - &service_data->keyexpr, - service_data->entity->topic_info()->topic_keyexpr_.c_str())) + &service_data->keyexpr, service_data->entity->topic_info()->topic_keyexpr_.c_str())) { 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); + 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(&qable_options); qable_options.complete = true; if (z_declare_queryable( - &service_data->qable, z_loan(context_impl->session), - z_loan(service_data->keyexpr), z_move(callback), &qable_options)) + &service_data->qable, z_loan(context_impl->session), z_loan(service_data->keyexpr), + z_move(callback), &qable_options)) { 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)); + }); std::string liveliness_keyexpr = service_data->entity->liveliness_keyexpr(); z_view_keyexpr_t liveliness_ke; z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); + auto free_token = rcpputils::make_scope_exit( + [service_data]() { + if (service_data != nullptr) { + z_drop(z_move(service_data->token)); + } + }); if (zc_liveliness_declare_token( - &service_data->token, - z_loan(node->context->impl->session), - z_loan(liveliness_ke), - NULL - )) + &service_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL)) { 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; } @@ -2926,24 +2883,28 @@ rmw_service_t * rmw_create_service( free_request_type_support.cancel(); free_response_type_support.cancel(); undeclare_z_queryable.cancel(); + free_token.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, + 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, + service, + service->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); @@ -2962,23 +2923,18 @@ rmw_ret_t rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) zc_liveliness_undeclare_token(z_move(service_data->token)); RMW_TRY_DESTRUCTOR( - service_data->request_type_support->~RequestTypeSupport(), - rmw_zenoh_cpp::RequestTypeSupport, ); + 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; @@ -2986,10 +2942,12 @@ rmw_ret_t rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) //============================================================================== /// Take an incoming ROS service request. -rmw_ret_t rmw_take_request( +rmw_ret_t +rmw_take_request( const rmw_service_t * service, rmw_service_info_t * request_header, - void * ros_request, bool * taken) + void * ros_request, + bool * taken) { *taken = false; @@ -2999,48 +2957,42 @@ rmw_ret_t rmw_take_request( RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, service->implementation_identifier, + 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); + 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); + 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_loaned_query_t * loaned_query = query->get_query(); - // DESERIALIZE MESSAGE - // ======================================================== + // 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(z_slice_data(z_loan(payload)))), + 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; @@ -3054,21 +3006,21 @@ rmw_ret_t rmw_take_request( request_header->request_id.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; @@ -3078,12 +3030,8 @@ rmw_ret_t 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; } @@ -3096,7 +3044,8 @@ rmw_ret_t rmw_take_request( //============================================================================== /// Send a ROS service response. -rmw_ret_t rmw_send_response( +rmw_ret_t +rmw_send_response( const rmw_service_t * service, rmw_request_id_t * request_header, void * ros_response) @@ -3107,7 +3056,8 @@ rmw_ret_t rmw_send_response( RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, service->implementation_identifier, + service, + service->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); @@ -3130,19 +3080,19 @@ rmw_ret_t rmw_send_response( rcutils_allocator_t * allocator = &(service_data->context->options.allocator); - size_t max_data_length = - (service_data->response_type_support->get_estimated_serialized_size( + 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( + auto free_response_bytes = rcpputils::make_scope_exit( [response_bytes, allocator]() { allocator->deallocate(response_bytes, allocator->state); }); @@ -3153,7 +3103,8 @@ rmw_ret_t 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(), + ros_response, + ser.get_cdr(), service_data->response_type_support_impl)) { return RMW_RET_ERROR; @@ -3167,8 +3118,7 @@ rmw_ret_t rmw_send_response( z_owned_bytes_t attachment; if (!create_map_and_set_sequence_num( - &attachment, - request_header->sequence_number, request_header->writer_guid)) + &attachment, request_header->sequence_number, request_header->writer_guid)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; @@ -3179,10 +3129,8 @@ rmw_ret_t rmw_send_response( 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); + loaned_query, z_loan(service_data->keyexpr), z_move(payload), &options); - z_drop(z_move(payload)); return RMW_RET_OK; } @@ -3196,7 +3144,8 @@ rmw_service_request_subscription_get_actual_qos( { RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, service->implementation_identifier, + 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); @@ -3222,7 +3171,8 @@ rmw_service_response_publisher_get_actual_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) +rmw_guard_condition_t * +rmw_create_guard_condition(rmw_context_t * context) { rcutils_allocator_t * allocator = &context->options.allocator; @@ -3233,34 +3183,31 @@ rmw_guard_condition_t * rmw_create_guard_condition(rmw_context_t * context) guard_condition, "unable to allocate memory for guard_condition", return nullptr); - auto free_guard_condition = - rcpputils::make_scope_exit( + 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->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( + "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( + 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(), + static_cast(guard_condition->data)->~GuardCondition(), rmw_zenoh_cpp::GuardCondition); }); @@ -3271,17 +3218,16 @@ rmw_guard_condition_t * 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; if (guard_condition->data) { - static_cast(guard_condition->data) - ->~GuardCondition(); + static_cast(guard_condition->data)->~GuardCondition(); allocator->deallocate(guard_condition->data, allocator->state); } @@ -3301,23 +3247,22 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition) 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, + context, + context->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); @@ -3326,7 +3271,8 @@ rmw_wait_set_t * rmw_create_wait_set( 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", + wait_set, + "failed to allocate wait set", return nullptr); auto cleanup_wait_set = rcpputils::make_scope_exit( [wait_set, allocator]() { @@ -3336,26 +3282,27 @@ rmw_wait_set_t * rmw_create_wait_set( 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); + 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; + 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(), + 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(); @@ -3366,7 +3313,8 @@ rmw_wait_set_t * rmw_create_wait_set( //============================================================================== /// 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); @@ -3376,8 +3324,7 @@ rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t * wait_set) 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; @@ -3391,18 +3338,19 @@ rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t * wait_set) namespace { -bool check_and_attach_condition( +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_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]); + static_cast(guard_conditions->guard_conditions[i]); if (gc == nullptr) { continue; } @@ -3419,21 +3367,17 @@ bool check_and_attach_condition( 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.", + "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; } } @@ -3441,8 +3385,8 @@ bool 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; } @@ -3454,14 +3398,11 @@ bool 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; } } @@ -3474,9 +3415,7 @@ bool check_and_attach_condition( 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; } } @@ -3488,91 +3427,86 @@ bool check_and_attach_condition( //============================================================================== /// Waits on sets of different entities and returns when one is ready. -rmw_ret_t rmw_wait( +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, + 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, + 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); RMW_CHECK_FOR_NULL_WITH_MSG( - wait_set_data, "waitset data struct is null", + 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. + // 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)), + 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]); + 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; @@ -3583,8 +3517,7 @@ rmw_ret_t 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; } @@ -3595,9 +3528,7 @@ rmw_ret_t rmw_wait( 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 { @@ -3608,8 +3539,8 @@ rmw_ret_t 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; } @@ -3625,8 +3556,7 @@ rmw_ret_t 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; } @@ -3662,7 +3592,8 @@ rmw_ret_t rmw_wait( //============================================================================== /// Return the name and namespace of all nodes in the ROS graph. -rmw_ret_t rmw_get_node_names( +rmw_ret_t +rmw_get_node_names( const rmw_node_t * node, rcutils_string_array_t * node_names, rcutils_string_array_t * node_namespaces) @@ -3682,9 +3613,12 @@ rmw_ret_t rmw_get_node_names( //============================================================================== /// 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); @@ -3702,28 +3636,27 @@ rmw_ret_t rmw_get_node_names_with_enclaves( //============================================================================== /// 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, +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, + 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); @@ -3733,61 +3666,57 @@ rmw_ret_t 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, +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, + 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, +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, + 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); @@ -3797,28 +3726,27 @@ rmw_ret_t 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, +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, + 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); @@ -3828,9 +3756,8 @@ rmw_ret_t 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); @@ -3847,7 +3774,8 @@ rmw_ret_t rmw_get_gid_for_publisher( //============================================================================== /// 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); @@ -3863,18 +3791,19 @@ rmw_ret_t 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, + 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, + 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); @@ -3886,14 +3815,16 @@ rmw_ret_t rmw_compare_gids_equal( //============================================================================== /// Check if a service server is available for the given service client. -rmw_ret_t rmw_service_server_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, + 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); @@ -3904,8 +3835,7 @@ rmw_ret_t rmw_service_server_is_available( 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; } @@ -3915,7 +3845,8 @@ rmw_ret_t 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", + "rmw_zenoh_cpp", + "Unexpected type %s for client %s. Report this bug", service_type.c_str(), client->service_name); return RMW_RET_INVALID_ARGUMENT; } @@ -3926,7 +3857,8 @@ rmw_ret_t rmw_service_server_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: @@ -3962,13 +3894,15 @@ rmw_subscription_set_on_new_message_callback( 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_ret_t +rmw_service_set_on_new_request_callback( rmw_service_t * service, rmw_event_callback_t callback, const void * user_data) @@ -3977,13 +3911,15 @@ rmw_ret_t rmw_service_set_on_new_request_callback( 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_ret_t +rmw_client_set_on_new_response_callback( rmw_client_t * client, rmw_event_callback_t callback, const void * user_data) @@ -3992,7 +3928,8 @@ rmw_ret_t rmw_client_set_on_new_response_callback( 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" From c7c17a6f40cec908d7582792ffdf3e5b661c989f Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 10 Sep 2024 14:10:38 +0800 Subject: [PATCH 48/77] chore(styel): align detail/rmw_data_types.cpp --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 138 ++++++++++---------- 1 file changed, 71 insertions(+), 67 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 31503a35..48161fc9 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -51,17 +51,21 @@ size_t hash_gid(const rmw_request_id_t & request_id) } // 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 { ///============================================================================= saved_msg_data::saved_msg_data( - z_owned_slice_t p, uint64_t recv_ts, + 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) + 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); } @@ -118,13 +122,11 @@ 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; @@ -144,25 +146,24 @@ void rmw_subscription_data_t::add_new_message( "rmw_zenoh_cpp", "Message queue depth of %ld reached, discarding oldest message " "for subscription for %s", - adapted_qos_profile.depth, topic_name.c_str()); + 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 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; @@ -179,8 +180,7 @@ 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(); } @@ -247,28 +247,26 @@ void rmw_service_data_t::add_new_query(std::unique_ptr query) "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))); + 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) + 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; @@ -284,29 +282,26 @@ 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; @@ -347,7 +342,8 @@ void rmw_client_data_t::add_new_reply(std::unique_ptr reply) "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))); + adapted_qos_profile.depth, + z_string_data(z_loan(keystr))); reply_queue_.pop_front(); } reply_queue_.emplace_back(std::move(reply)); @@ -396,8 +392,8 @@ 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. +// 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_); @@ -405,8 +401,8 @@ 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. +// 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_); @@ -418,8 +414,7 @@ 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) +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; @@ -433,7 +428,9 @@ bool rmw_client_data_t::is_shutdown() const } //============================================================================== -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); @@ -455,7 +452,8 @@ void sub_data_handler(const z_loaned_sample_t * sample, void * data) // 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 = @@ -465,12 +463,10 @@ void sub_data_handler(const z_loaned_sample_t * sample, void * data) // 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. @@ -487,9 +483,11 @@ void sub_data_handler(const z_loaned_sample_t * sample, void * data) sub_data->add_new_message( std::make_unique( - slice, 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))); + z_string_data(z_loan(keystr))); } ///============================================================================= @@ -499,7 +497,10 @@ 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 @@ -527,16 +528,21 @@ 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 { + // z_reply_ok return a null pointer if not z_reply_is_ok return z_reply_ok(z_loan(reply_)); } @@ -554,12 +560,13 @@ void client_data_handler(const z_loaned_reply_t * reply, void * data) if (client_data == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", - "Unable to obtain client_data_t "); + "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; } @@ -574,7 +581,6 @@ void client_data_handler(const z_loaned_reply_t * reply, void * data) 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( @@ -593,21 +599,19 @@ void client_data_drop(void * data) if (client_data == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", - "Unable to obtain client_data_t "); + "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); } From 1eef11d3f4ea40d1280744418a686b9c8d04db37 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 10 Sep 2024 14:39:27 +0800 Subject: [PATCH 49/77] chore(style): align rmw_init.cpp --- rmw_zenoh_cpp/src/rmw_init.cpp | 173 ++++++++++++++++----------------- 1 file changed, 82 insertions(+), 91 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index e12ef24b..21afc8ff 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -31,14 +31,14 @@ #include "rcutils/strdup.h" #include "rcutils/types.h" -#include "rmw/impl/cpp/macros.hpp" #include "rmw/init.h" +#include "rmw/impl/cpp/macros.hpp" #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 @@ -54,7 +54,8 @@ graph_sub_data_handler(const z_loaned_sample_t * sample, void * data) 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", @@ -76,8 +77,7 @@ graph_sub_data_handler(const z_loaned_sample_t * sample, void * data) 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,7 +89,8 @@ 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) +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); @@ -98,11 +99,13 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) "expected initialized init options", return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - options, options->implementation_identifier, + 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", + 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"); @@ -114,31 +117,28 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * 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)); + 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", + 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); + 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 *); + context->impl->~rmw_context_impl_t(), rmw_context_impl_t *); }); rmw_ret_t ret; @@ -150,8 +150,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) [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"); + RMW_SAFE_FWRITE_TO_STDERR("Failed to cleanup context options during error handling"); } }); @@ -179,16 +178,16 @@ rmw_ret_t 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; } - - // TODO(yuyuan): SHM + // Check if shm is enabled. 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( @@ -203,7 +202,9 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) } 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)); @@ -216,12 +217,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) 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)); @@ -234,13 +231,11 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) } } - - // 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"); + RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled"); - // TODO(yuyuan): determine the default alignment + // TODO(yuyuan): determine the default alignment of SHM z_alloc_alignment_t alignment = {5}; z_owned_memory_layout_t layout; z_memory_layout_new(&layout, SHM_BUFFER_SIZE_MB * 1024 * 1024, alignment); @@ -252,7 +247,6 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) } context->impl->shm_provider = std::make_optional(provider); } - auto free_shm_provider = rcpputils::make_scope_exit( [context]() { if (context->impl->shm_provider.has_value()) { @@ -268,29 +262,23 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) context->impl->graph_guard_condition, "failed to allocate graph guard condition", return RMW_RET_BAD_ALLOC); - auto free_guard_condition = - rcpputils::make_scope_exit( + auto free_guard_condition = rcpputils::make_scope_exit( [context, allocator]() { - allocator->deallocate( - context->impl->graph_guard_condition, - allocator->state); + 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); + 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( + auto free_guard_condition_data = rcpputils::make_scope_exit( [context, allocator]() { - allocator->deallocate( - context->impl->graph_guard_condition->data, - allocator->state); + allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); }); RMW_TRY_PLACEMENT_NEW( @@ -300,46 +288,44 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) 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); + 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. + // 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); + z_fifo_channel_reply_new(&closure, &handler, SIZE_MAX); 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; + z_owned_reply_t reply; while (z_recv(z_loan(handler), &reply) == Z_OK) { if (z_reply_is_ok(z_loan(reply))) { const z_loaned_sample_t * sample = z_reply_ok(z_loan(reply)); @@ -352,16 +338,15 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) } else { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "[discovery] Received an error\n"); } + z_drop(z_move(reply)); } - z_drop(z_move(reply)); z_drop(z_move(handler)); - // 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( @@ -376,6 +361,10 @@ 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()); + auto undeclare_z_sub = rcpputils::make_scope_exit( + [context]() { + z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); + }); if (zc_liveliness_declare_subscriber( &context->impl->graph_subscriber, z_loan(context->impl->session), z_loan(keyexpr), @@ -385,6 +374,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_ERROR; } + undeclare_z_sub.cancel(); close_session.cancel(); destruct_guard_condition_data.cancel(); impl_destructor.cancel(); @@ -402,14 +392,17 @@ rmw_ret_t 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", + context->impl, + "expected initialized context", return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - context, context->implementation_identifier, + context, + context->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); @@ -430,14 +423,17 @@ rmw_ret_t 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", + context->impl, + "expected initialized context", return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - context, context->implementation_identifier, + context, + context->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); if (!context->impl->is_shutdown) { @@ -449,21 +445,16 @@ rmw_ret_t rmw_context_fini(rmw_context_t * context) RMW_TRY_DESTRUCTOR( static_cast( - context->impl->graph_guard_condition->data) - ->~GuardCondition(), + 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->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); From e57ce89ee7cd3e06fd45c281e679a9a2b46f3318 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 10 Sep 2024 14:41:49 +0800 Subject: [PATCH 50/77] chore(style): align detail/attachment_helpers.cpp --- rmw_zenoh_cpp/src/detail/attachment_helpers.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index fe100f21..9a967a12 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -161,5 +161,4 @@ int64_t get_int64_from_attachment( return num; } - } // namespace rmw_zenoh_cpp From 86940a807987ab2c0f34a92f17b774a24892c912 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 10 Sep 2024 14:42:10 +0800 Subject: [PATCH 51/77] build: update zenoh-c version --- 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 33e7e730..6e648da3 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -26,7 +26,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - 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 8d5b11ce114a4eb591f15891878badeb53e56c48 + VCS_VERSION ecad7f358fabdf55c4d6c35118415b5c457c8f20 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" From 3aba418e9dd42481f1b2999c8e0fd9fac493c688 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 10 Sep 2024 15:20:41 +0800 Subject: [PATCH 52/77] chore(style): align the remaining files --- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 1 - rmw_zenoh_cpp/src/rmw_zenoh.cpp | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 7f3dbe6e..7780df35 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -282,7 +282,6 @@ class ZenohReply final ~ZenohReply(); - // TODO(yuyuan): rename this function const z_loaned_sample_t * get_sample() const; private: diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index eb991a98..35509034 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -2498,7 +2498,7 @@ rmw_take_response( } const z_loaned_sample_t * sample = latest_reply->get_sample(); - if (!sample) { + if (sample == NULL) { RMW_SET_ERROR_MSG("invalid reply sample"); return RMW_RET_ERROR; } From 05ff44320220b9c825400153b237df87386d9f33 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 10 Sep 2024 15:29:56 +0800 Subject: [PATCH 53/77] chore(style): consistently use `Z_OK` in the if condition --- .../src/detail/attachment_helpers.cpp | 2 +- .../src/detail/zenoh_router_check.cpp | 2 +- rmw_zenoh_cpp/src/rmw_init.cpp | 6 ++-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 30 +++++++++---------- rmw_zenoh_cpp/src/zenohd/main.cpp | 2 +- 5 files changed, 21 insertions(+), 21 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 9a967a12..b2e0b289 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -148,7 +148,7 @@ int64_t get_int64_from_attachment( } int64_t num; - if (z_bytes_deserialize_into_int64(z_loan(val), &num)) { + if (z_bytes_deserialize_into_int64(z_loan(val), &num) != Z_OK) { return -1; } diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp index 74081fb9..150cf111 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp @@ -43,7 +43,7 @@ rmw_ret_t zenoh_router_check(const z_loaned_session_t * session) rmw_ret_t ret = RMW_RET_OK; z_owned_closure_zid_t router_callback; z_closure(&router_callback, callback, NULL, &context); - if (z_info_routers_zid(session, z_move(router_callback))) { + if (z_info_routers_zid(session, z_move(router_callback)) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Failed to evaluate if Zenoh routers are connected to the session."); diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 21afc8ff..02aa671a 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -196,7 +196,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) }); // Initialize the zenoh session. - if (z_open(&context->impl->session, z_move(config))) { + if (z_open(&context->impl->session, z_move(config)) != Z_OK) { RMW_SET_ERROR_MSG("Error setting up zenoh session"); return RMW_RET_ERROR; } @@ -241,7 +241,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) 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))) { + if (z_posix_shm_provider_new(&provider, z_loan(layout)) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a SHM provider."); return RMW_RET_ERROR; } @@ -411,7 +411,7 @@ rmw_shutdown(rmw_context_t * context) z_drop(z_move(context->impl->shm_provider.value())); } // Close the zenoh session - if (z_close(z_move(context->impl->session)) < 0) { + if (z_close(z_move(context->impl->session)) != Z_OK) { RMW_SET_ERROR_MSG("Error while closing zenoh session"); return RMW_RET_ERROR; } diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 35509034..2c800aba 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -273,7 +273,7 @@ rmw_create_node( z_drop(z_move(node_data->token)); }); if (zc_liveliness_declare_token( - &node_data->token, z_loan(context->impl->session), z_loan(keyexpr), NULL)) + &node_data->token, z_loan(context->impl->session), z_loan(keyexpr), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -641,7 +641,7 @@ rmw_create_publisher( } // 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(pub_ke), &opts)) + &publisher_data->pub, z_loan(context_impl->session), z_loan(pub_ke), &opts) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh publisher"); return nullptr; @@ -662,7 +662,7 @@ rmw_create_publisher( }); if (zc_liveliness_declare_token( &publisher_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), - NULL)) + NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -715,7 +715,7 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) } 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))) { + if (z_undeclare_publisher(z_move(publisher_data->pub)) != Z_OK) { RMW_SET_ERROR_MSG("failed to undeclare pub"); ret = RMW_RET_ERROR; } @@ -940,7 +940,7 @@ rmw_publish( 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)) { + 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; } @@ -1069,7 +1069,7 @@ rmw_publish_serialized_message( 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)) { + 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; } @@ -1400,7 +1400,7 @@ rmw_create_subscription( std::string topic_keyexpr = sub_data->entity->topic_info()->topic_keyexpr_; z_view_keyexpr_t sub_ke; - if (z_view_keyexpr_from_str(&sub_ke, topic_keyexpr.c_str())) { + if (z_view_keyexpr_from_str(&sub_ke, topic_keyexpr.c_str()) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } @@ -1474,7 +1474,7 @@ rmw_create_subscription( z_owned_subscriber_t sub; if (z_declare_subscriber( - &sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), &sub_options)) + &sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), &sub_options) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; @@ -1507,7 +1507,7 @@ rmw_create_subscription( } }); if (zc_liveliness_declare_token( - &sub_data->token, z_loan(context_impl->session), z_loan(liveliness_ke), NULL)) + &sub_data->token, z_loan(context_impl->session), z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -1561,7 +1561,7 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) z_owned_subscriber_t * sub = std::get_if(&sub_data->sub); if (sub != nullptr) { - if (z_undeclare_subscriber(z_move(*sub))) { + if (z_undeclare_subscriber(z_move(*sub)) != Z_OK) { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; } @@ -2251,7 +2251,7 @@ rmw_create_client( z_keyexpr_drop(z_move(client_data->keyexpr)); }); if (z_keyexpr_from_str( - &client_data->keyexpr, client_data->entity->topic_info()->topic_keyexpr_.c_str())) + &client_data->keyexpr, client_data->entity->topic_info()->topic_keyexpr_.c_str()) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; @@ -2267,7 +2267,7 @@ rmw_create_client( } }); if (zc_liveliness_declare_token( - &client_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL)) + &client_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -2830,7 +2830,7 @@ rmw_create_service( return nullptr; } if (z_keyexpr_from_str( - &service_data->keyexpr, service_data->entity->topic_info()->topic_keyexpr_.c_str())) + &service_data->keyexpr, service_data->entity->topic_info()->topic_keyexpr_.c_str()) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; @@ -2844,7 +2844,7 @@ rmw_create_service( qable_options.complete = true; if (z_declare_queryable( &service_data->qable, z_loan(context_impl->session), z_loan(service_data->keyexpr), - z_move(callback), &qable_options)) + z_move(callback), &qable_options) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh queryable"); return nullptr; @@ -2864,7 +2864,7 @@ rmw_create_service( } }); if (zc_liveliness_declare_token( - &service_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL)) + &service_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", diff --git a/rmw_zenoh_cpp/src/zenohd/main.cpp b/rmw_zenoh_cpp/src/zenohd/main.cpp index 2dd489f5..ab038ddf 100644 --- a/rmw_zenoh_cpp/src/zenohd/main.cpp +++ b/rmw_zenoh_cpp/src/zenohd/main.cpp @@ -79,7 +79,7 @@ int main(int argc, char ** argv) } z_owned_session_t session; - if (z_open(&session, z_move(config))) { + if (z_open(&session, z_move(config)) != Z_OK) { printf("Unable to open router session!\n"); return 1; } From dd1e6912e394a3ab5cb9a8055ffe7466968a2439 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 10 Sep 2024 15:32:48 +0800 Subject: [PATCH 54/77] chore(style): ament_uncrustify --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 2 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 19 +++++++++++-------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 48161fc9..845a11a3 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -487,7 +487,7 @@ void sub_data_handler( z_timestamp_ntp64_time(z_sample_timestamp(sample)), pub_gid, sequence_number, source_timestamp), - z_string_data(z_loan(keystr))); + z_string_data(z_loan(keystr))); } ///============================================================================= diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 2c800aba..67c05a04 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1053,9 +1053,9 @@ rmw_publish_serialized_message( } auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() { - z_drop(z_move(attachment)); - }); + [&attachment]() { + z_drop(z_move(attachment)); + }); const size_t data_length = ser.get_serialized_data_length(); @@ -1474,7 +1474,8 @@ rmw_create_subscription( z_owned_subscriber_t sub; if (z_declare_subscriber( - &sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), &sub_options) != Z_OK) + &sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), + &sub_options) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; @@ -2267,7 +2268,8 @@ rmw_create_client( } }); if (zc_liveliness_declare_token( - &client_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL) != Z_OK) + &client_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), + NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -2864,7 +2866,8 @@ rmw_create_service( } }); if (zc_liveliness_declare_token( - &service_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), NULL) != Z_OK) + &service_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), + NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -2991,8 +2994,8 @@ rmw_take_request( 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)) + ros_request, + service_data->request_type_support_impl)) { RMW_SET_ERROR_MSG("could not deserialize ROS message"); return RMW_RET_ERROR; From 5bf7cc035cbc925197a2922f84c0121834c5a0c6 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 10 Sep 2024 15:36:36 +0800 Subject: [PATCH 55/77] chore(style): ament_cpplint --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 67c05a04..e5d9dbb4 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -108,7 +108,6 @@ const rosidl_service_type_support_t * find_service_type_support( extern "C" { - // TODO(yuyuan): SHM, make this configurable #define SHM_BUF_OK_SIZE 2621440 From 206e34e17620998ef735fab787d8a23db38c28f5 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 11 Sep 2024 16:08:55 +0800 Subject: [PATCH 56/77] fix: set the max size of initial query queue to `SIZE_MAX - 1` --- rmw_zenoh_cpp/src/rmw_init.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 02aa671a..b32d35d6 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -317,7 +317,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // 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; - z_fifo_channel_reply_new(&closure, &handler, SIZE_MAX); + z_fifo_channel_reply_new(&closure, &handler, SIZE_MAX - 1); z_view_keyexpr_t keyexpr; z_view_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); From e9d05133e8f7787a516a6ae9758652e21b3185bf Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Mon, 16 Sep 2024 15:37:56 +0800 Subject: [PATCH 57/77] fix: iterator memory leak --- rmw_zenoh_cpp/src/detail/attachment_helpers.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index b2e0b289..9ed51c1e 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -64,7 +64,7 @@ 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)) { + if (attachment == NULL || z_bytes_is_empty(attachment)) { return false; } @@ -89,6 +89,8 @@ bool get_attachment( if (found) { break; + } else { + z_drop(z_move(*val)); } } @@ -107,7 +109,7 @@ 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)) { + if (attachment == NULL || z_bytes_is_empty(attachment)) { return false; } @@ -135,7 +137,7 @@ int64_t get_int64_from_attachment( const std::string & name) { // A valid request must have had an attachment - if (z_bytes_is_empty(attachment)) { + if (attachment == NULL || z_bytes_is_empty(attachment)) { return -1; } From 520a3a2f255f6782cf10083680ec2551c7147c40 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 13 Sep 2024 23:58:43 +0200 Subject: [PATCH 58/77] feat: update to zenoh-c 1.0.0.8 changes --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 6 +++--- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 6 +++--- rmw_zenoh_cpp/src/rmw_init.cpp | 8 ++++---- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 4 +++- rmw_zenoh_cpp/src/zenohd/main.cpp | 4 ++-- zenoh_c_vendor/CMakeLists.txt | 2 +- 6 files changed, 16 insertions(+), 14 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 845a11a3..e968bd03 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -429,7 +429,7 @@ bool rmw_client_data_t::is_shutdown() const //============================================================================== void sub_data_handler( - const z_loaned_sample_t * sample, + z_loaned_sample_t * sample, void * data) { z_view_string_t keystr; @@ -509,7 +509,7 @@ const z_loaned_query_t * ZenohQuery::get_query() const } //============================================================================== -void service_data_handler(const z_loaned_query_t * query, void * data) +void service_data_handler(z_loaned_query_t * query, void * data) { z_view_string_t keystr; z_keyexpr_as_view_string(z_query_keyexpr(query), &keystr); @@ -554,7 +554,7 @@ size_t rmw_client_data_t::get_next_sequence_number() } //============================================================================== -void client_data_handler(const z_loaned_reply_t * reply, void * data) +void client_data_handler(z_loaned_reply_t * reply, void * data) { auto client_data = static_cast(data); if (client_data == nullptr) { diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 7780df35..b39728e2 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -129,7 +129,7 @@ class rmw_publisher_data_t final ///============================================================================= // z_owned_closure_sample_t -void sub_data_handler(const z_loaned_sample_t * sample, void * sub_data); +void sub_data_handler(z_loaned_sample_t * sample, void * sub_data); struct saved_msg_data { @@ -198,10 +198,10 @@ class rmw_subscription_data_t final ///============================================================================= -void service_data_handler(const z_loaned_query_t * query, void * service_data); +void service_data_handler(z_loaned_query_t * query, void * service_data); ///============================================================================= -void client_data_handler(const z_loaned_reply_t * reply, void * client_data); +void client_data_handler(z_loaned_reply_t * reply, void * client_data); 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 b32d35d6..705da0c3 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -46,7 +46,7 @@ extern "C" namespace { void -graph_sub_data_handler(const z_loaned_sample_t * sample, void * data) +graph_sub_data_handler(z_loaned_sample_t * sample, void * data) { static_cast(data); @@ -196,14 +196,14 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) }); // Initialize the zenoh session. - if (z_open(&context->impl->session, z_move(config)) != Z_OK) { + if (z_open(&context->impl->session, z_move(config), NULL) != Z_OK) { 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)); + z_close(z_move(context->impl->session), NULL); }); /// Initialize the graph cache. @@ -411,7 +411,7 @@ rmw_shutdown(rmw_context_t * context) z_drop(z_move(context->impl->shm_provider.value())); } // Close the zenoh session - if (z_close(z_move(context->impl->session)) != Z_OK) { + if (z_close(z_move(context->impl->session), NULL) != Z_OK) { RMW_SET_ERROR_MSG("Error while closing zenoh session"); return RMW_RET_ERROR; } diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index e5d9dbb4..14789faa 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1426,7 +1426,9 @@ rmw_create_subscription( // by various publishers. sub_options.query_consolidation = z_query_consolidation_none(); if (sub_data->adapted_qos_profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { - sub_options.reliability = Z_RELIABILITY_RELIABLE; + RMW_ZENOH_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "`reliability` no longer supported on subscriber. `reliability` on `declare_publisher` or `put`. Ignoring..."); } ze_owned_querying_subscriber_t sub; diff --git a/rmw_zenoh_cpp/src/zenohd/main.cpp b/rmw_zenoh_cpp/src/zenohd/main.cpp index ab038ddf..1a4fdf6e 100644 --- a/rmw_zenoh_cpp/src/zenohd/main.cpp +++ b/rmw_zenoh_cpp/src/zenohd/main.cpp @@ -79,13 +79,13 @@ int main(int argc, char ** argv) } z_owned_session_t session; - if (z_open(&session, z_move(config)) != Z_OK) { + if (z_open(&session, z_move(config), NULL) != Z_OK) { printf("Unable to open router session!\n"); return 1; } auto always_close_session = rcpputils::make_scope_exit( [&session]() { - z_close(z_move(session)); + z_close(z_move(session), NULL); }); printf( diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 6e648da3..f0737d1e 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -26,7 +26,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - 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 ecad7f358fabdf55c4d6c35118415b5c457c8f20 + VCS_VERSION 1.0.0.8 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" From fce8a629e0d6ef1a9f67c21da9c2321ee2b06ceb Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Mon, 16 Sep 2024 16:17:56 +0800 Subject: [PATCH 59/77] chore(style): address `ament_cpplint` and `ament_uncrustiy` --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 14789faa..bd71b266 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1427,8 +1427,8 @@ rmw_create_subscription( sub_options.query_consolidation = z_query_consolidation_none(); if (sub_data->adapted_qos_profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { RMW_ZENOH_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "`reliability` no longer supported on subscriber. `reliability` on `declare_publisher` or `put`. Ignoring..."); + "rmw_zenoh_cpp", + "`reliability` no longer supported on subscriber. Ignoring..."); } ze_owned_querying_subscriber_t sub; From 09c5cbc394efc39e99aede7567cad67c122842b9 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Tue, 17 Sep 2024 18:28:19 +0800 Subject: [PATCH 60/77] fix: initiate zenoh logger --- rmw_zenoh_cpp/src/rmw_init.cpp | 2 ++ rmw_zenoh_cpp/src/zenohd/main.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 705da0c3..7cc221de 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -176,6 +176,8 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_ERROR; } + zc_try_init_log_from_env(); + // Initialize the zenoh configuration. z_owned_config_t config; if ((ret = diff --git a/rmw_zenoh_cpp/src/zenohd/main.cpp b/rmw_zenoh_cpp/src/zenohd/main.cpp index 1a4fdf6e..cf97e97a 100644 --- a/rmw_zenoh_cpp/src/zenohd/main.cpp +++ b/rmw_zenoh_cpp/src/zenohd/main.cpp @@ -68,6 +68,8 @@ int main(int argc, char ** argv) return 1; } + zc_try_init_log_from_env(); + // Initialize the zenoh configuration for the router. z_owned_config_t config; if ((rmw_zenoh_cpp::get_z_config( From 54dd96a8da63e24f971fe57220d3dd045df91136 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 19 Sep 2024 23:17:05 +0800 Subject: [PATCH 61/77] chore: apply the suggestions --- .../src/detail/attachment_helpers.cpp | 18 ++++++++- .../src/detail/attachment_helpers.hpp | 21 ++++------- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 1 + rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 37 +++++++++++-------- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 12 +++--- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 2 +- 6 files changed, 54 insertions(+), 37 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 9ed51c1e..6183b96c 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "rmw/types.h" @@ -27,6 +28,9 @@ namespace rmw_zenoh_cpp { +attachement_context_t::attachement_context_t(std::unique_ptr && _data) +: data(std::move(_data)) {} + bool create_attachment_iter(z_owned_bytes_t * kv_pair, void * context) { attachement_context_t * ctx = reinterpret_cast(context); @@ -52,14 +56,26 @@ bool create_attachment_iter(z_owned_bytes_t * kv_pair, void * context) return true; } +attachement_data_t::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_result_t attachement_data_t::serialize_to_zbytes(z_owned_bytes_t * attachment) { - attachement_context_t context = attachement_context_t(this); + attachement_context_t context = + attachement_context_t(std::make_unique(*this)); return z_bytes_from_iter( attachment, create_attachment_iter, reinterpret_cast(&context)); } + bool get_attachment( const z_loaned_bytes_t * const attachment, const std::string & key, z_owned_bytes_t * val) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp index d01d3e8f..c31714c1 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -18,6 +18,7 @@ #include #include +#include #include "rmw/types.h" @@ -30,27 +31,21 @@ class attachement_data_t final int64_t sequence_number; int64_t source_timestamp; uint8_t source_gid[RMW_GID_STORAGE_SIZE]; - attachement_data_t( + explicit 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); - } + const uint8_t _source_gid[RMW_GID_STORAGE_SIZE]); z_result_t serialize_to_zbytes(z_owned_bytes_t *); }; class attachement_context_t final { public: - const attachement_data_t * data; + std::unique_ptr data; int length = 3; int idx = 0; - attachement_context_t(const attachement_data_t * _data) - : data(_data) {} + attachement_context_t(std::unique_ptr && _data); }; bool get_attachment( @@ -58,12 +53,10 @@ bool get_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]); + 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); + 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/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index 4ad19def..c0be54f2 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "liveliness_utils.hpp" + #include #include diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index e968bd03..6d44d899 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -69,6 +69,7 @@ saved_msg_data::saved_msg_data( { memcpy(publisher_gid, pub_gid, RMW_GID_STORAGE_SIZE); } + ///============================================================================= saved_msg_data::~saved_msg_data() { @@ -441,7 +442,8 @@ void sub_data_handler( "rmw_zenoh_cpp", "Unable to obtain rmw_subscription_data_t from data for " "subscription for %s", - z_string_data(z_loan(keystr))); + z_string_data(z_loan(keystr)) + ); return; } @@ -456,8 +458,7 @@ void sub_data_handler( "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. @@ -486,26 +487,27 @@ void sub_data_handler( slice, z_timestamp_ntp64_time(z_sample_timestamp(sample)), pub_gid, - sequence_number, source_timestamp), + sequence_number, + source_timestamp), z_string_data(z_loan(keystr))); } ///============================================================================= -ZenohQuery::ZenohQuery(const z_loaned_query_t * query) +ZenohQuery::ZenohQuery(z_owned_query_t * query) { - z_query_clone(&query_, query); + query_ = query; } ///============================================================================= ZenohQuery::~ZenohQuery() { - z_drop(z_move(query_)); + z_drop(z_move(*query_)); } ///============================================================================= const z_loaned_query_t * ZenohQuery::get_query() const { - return z_query_loan(&query_); + return z_loan(*query_); } //============================================================================== @@ -514,7 +516,8 @@ void service_data_handler(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", @@ -524,26 +527,28 @@ void service_data_handler(z_loaned_query_t * query, void * data) return; } - service_data->add_new_query(std::make_unique(query)); + z_owned_query_t owned_query; + z_query_clone(&owned_query, query); + + service_data->add_new_query(std::make_unique(&owned_query)); } ///============================================================================= -ZenohReply::ZenohReply(const z_owned_reply_t * reply) +ZenohReply::ZenohReply(z_owned_reply_t * reply) { - reply_ = *reply; + reply_ = reply; } ///============================================================================= ZenohReply::~ZenohReply() { - z_reply_drop(z_move(reply_)); + z_drop(z_move(*reply_)); } ///============================================================================= -const z_loaned_sample_t * ZenohReply::get_sample() const +const z_loaned_reply_t * ZenohReply::get_reply() const { - // z_reply_ok return a null pointer if not z_reply_is_ok - return z_reply_ok(z_loan(reply_)); + return z_loan(*reply_); } ///============================================================================= diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index b39728e2..23ddcb2b 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -52,6 +52,8 @@ class rmw_context_impl_s final // An owned session. z_owned_session_t session; + // An optional SHM provider that is initialized of SHM is enabled in the + // zenoh session config. std::optional shm_provider; z_owned_subscriber_t graph_subscriber; @@ -208,14 +210,14 @@ void client_data_drop(void * data); class ZenohQuery final { public: - ZenohQuery(const z_loaned_query_t * query); + ZenohQuery(z_owned_query_t * query); ~ZenohQuery(); const z_loaned_query_t * get_query() const; private: - z_owned_query_t query_; + z_owned_query_t * query_; }; ///============================================================================= @@ -278,14 +280,14 @@ class rmw_service_data_t final class ZenohReply final { public: - ZenohReply(const z_owned_reply_t * reply); + ZenohReply(z_owned_reply_t * reply); ~ZenohReply(); - const z_loaned_sample_t * get_sample() const; + const z_loaned_reply_t * get_reply() const; private: - z_owned_reply_t reply_; + z_owned_reply_t * reply_; }; ///============================================================================= diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index bd71b266..ae0df777 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -2500,7 +2500,7 @@ rmw_take_response( return RMW_RET_OK; } - const z_loaned_sample_t * sample = latest_reply->get_sample(); + const z_loaned_sample_t * sample = z_reply_ok(latest_reply->get_reply()); if (sample == NULL) { RMW_SET_ERROR_MSG("invalid reply sample"); return RMW_RET_ERROR; From becea930a4bf4ed612a7b4c4d0c9bc56f7d35148 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 19 Sep 2024 23:20:14 +0800 Subject: [PATCH 62/77] chore: add the comments for the zenoh logger --- rmw_zenoh_cpp/src/rmw_init.cpp | 1 + rmw_zenoh_cpp/src/zenohd/main.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 7cc221de..aa2b495c 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -176,6 +176,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_ERROR; } + // Enable the zenoh built-in logger zc_try_init_log_from_env(); // Initialize the zenoh configuration. diff --git a/rmw_zenoh_cpp/src/zenohd/main.cpp b/rmw_zenoh_cpp/src/zenohd/main.cpp index cf97e97a..8249da5f 100644 --- a/rmw_zenoh_cpp/src/zenohd/main.cpp +++ b/rmw_zenoh_cpp/src/zenohd/main.cpp @@ -68,6 +68,7 @@ int main(int argc, char ** argv) return 1; } + // Enable the zenoh built-in logger zc_try_init_log_from_env(); // Initialize the zenoh configuration for the router. From 1f135c25cac11bd12db5eb2467af26b1b0d42e8e Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Fri, 20 Sep 2024 23:32:06 +0800 Subject: [PATCH 63/77] fix: store and destroy the subscriber properly --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index f608c2ac..f2c4974c 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1438,6 +1438,8 @@ rmw_create_subscription( RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; } + sub_data->sub = sub; + // Register the querying subscriber with the graph cache to get latest // messages from publishers that were discovered after their first publication. context_impl->graph_cache->set_querying_subscriber_callback( @@ -1566,7 +1568,7 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) allocator->deallocate(sub_data->type_support, allocator->state); z_owned_subscriber_t * sub = std::get_if(&sub_data->sub); - if (sub != nullptr) { + if (sub != NULL) { if (z_undeclare_subscriber(z_move(*sub)) != Z_OK) { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; @@ -1574,7 +1576,7 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) } else { ze_owned_querying_subscriber_t * querying_sub = std::get_if(&sub_data->sub); - if (querying_sub == nullptr || ze_undeclare_querying_subscriber(z_move(*querying_sub))) { + if (querying_sub == NULL || ze_undeclare_querying_subscriber(z_move(*querying_sub))) { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; } From 51775941b697b019ea555928f39bcd07a72d46d0 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Sat, 21 Sep 2024 00:22:49 +0800 Subject: [PATCH 64/77] chore: improve the null pointer check: NULL => nullptr --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index f2c4974c..06355e34 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1568,7 +1568,7 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) allocator->deallocate(sub_data->type_support, allocator->state); z_owned_subscriber_t * sub = std::get_if(&sub_data->sub); - if (sub != NULL) { + if (sub != nullptr) { if (z_undeclare_subscriber(z_move(*sub)) != Z_OK) { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; @@ -1576,7 +1576,7 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) } else { ze_owned_querying_subscriber_t * querying_sub = std::get_if(&sub_data->sub); - if (querying_sub == NULL || ze_undeclare_querying_subscriber(z_move(*querying_sub))) { + if (querying_sub == nullptr || ze_undeclare_querying_subscriber(z_move(*querying_sub))) { RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; } From 6a48919727226ebad853373e8a1eec0ee22c3be4 Mon Sep 17 00:00:00 2001 From: Julien Enoch Date: Mon, 23 Sep 2024 17:15:21 +0200 Subject: [PATCH 65/77] Change liveliness tokens logs from warn to debug level (#22) --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 36f931f1..04f8fd04 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -595,8 +595,10 @@ void GraphCache::parse_del( return entity->zid() == node_it.second->zid_ && entity->nid() == node_it.second->nid_; }); if (node_it == range.second) { - // Node does not exist. - RMW_ZENOH_LOG_WARN_NAMED( + // Node does not exist or its liveliness token has been unregistered before one of its + // pubs/subs/service liveliness token. This could happen since Zenoh doesn't guarantee + // any order for unregistration events if the remote Node closed abruptly or was disconnected. + RMW_ZENOH_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", "Received liveliness token to remove unknown node /%s from the graph. Ignoring...", entity->node_name().c_str() @@ -606,16 +608,17 @@ void GraphCache::parse_del( if (entity->type() == EntityType::Node) { // Node - // The liveliness tokens to remove pub/subs should be received before the one to remove a node - // given the reliability QoS for liveliness subs. However, if we find any pubs/subs present in - // the node below, we should update the count in graph_topics_. + // In case the remote Node closed abruptly or was disconnected, Zenoh could deliver the + // liveliness tokens unregistration events in any order. + // If the event for Node unregistration comes before the unregistration of its + // pubs/subs/services, we should update the count in graph_topics_ and graph_services_. const GraphNodePtr graph_node = node_it->second; if (!graph_node->pubs_.empty() || !graph_node->subs_.empty() || !graph_node->clients_.empty() || !graph_node->services_.empty()) { - RMW_ZENOH_LOG_WARN_NAMED( + RMW_ZENOH_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", "Received liveliness token to remove node /%s from the graph before all pub/subs/" "clients/services for this node have been removed. Removing all entities first...", From 6fd7e1cfaf651ec53d6df8301271a62f56b78e44 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Fri, 27 Sep 2024 17:01:16 +0800 Subject: [PATCH 66/77] fix: properly clone the pointer of query and reply to resolve the segfault in test_service__rmw_zenoh_cpp --- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 16 ++++++++-------- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 8 ++++---- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 6d44d899..92804f9c 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -493,7 +493,7 @@ void sub_data_handler( } ///============================================================================= -ZenohQuery::ZenohQuery(z_owned_query_t * query) +ZenohQuery::ZenohQuery(z_owned_query_t query) { query_ = query; } @@ -501,13 +501,13 @@ ZenohQuery::ZenohQuery(z_owned_query_t * query) ///============================================================================= ZenohQuery::~ZenohQuery() { - z_drop(z_move(*query_)); + z_drop(z_move(query_)); } ///============================================================================= const z_loaned_query_t * ZenohQuery::get_query() const { - return z_loan(*query_); + return z_loan(query_); } //============================================================================== @@ -530,11 +530,11 @@ void service_data_handler(z_loaned_query_t * query, void * data) z_owned_query_t owned_query; z_query_clone(&owned_query, query); - service_data->add_new_query(std::make_unique(&owned_query)); + service_data->add_new_query(std::make_unique(owned_query)); } ///============================================================================= -ZenohReply::ZenohReply(z_owned_reply_t * reply) +ZenohReply::ZenohReply(z_owned_reply_t reply) { reply_ = reply; } @@ -542,13 +542,13 @@ ZenohReply::ZenohReply(z_owned_reply_t * reply) ///============================================================================= ZenohReply::~ZenohReply() { - z_drop(z_move(*reply_)); + z_drop(z_move(reply_)); } ///============================================================================= const z_loaned_reply_t * ZenohReply::get_reply() const { - return z_loan(*reply_); + return z_loan(reply_); } ///============================================================================= @@ -579,7 +579,7 @@ void client_data_handler(z_loaned_reply_t * reply, void * data) 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)); + 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); diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 23ddcb2b..e52d56d8 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -210,14 +210,14 @@ void client_data_drop(void * data); class ZenohQuery final { public: - ZenohQuery(z_owned_query_t * query); + ZenohQuery(z_owned_query_t query); ~ZenohQuery(); const z_loaned_query_t * get_query() const; private: - z_owned_query_t * query_; + z_owned_query_t query_; }; ///============================================================================= @@ -280,14 +280,14 @@ class rmw_service_data_t final class ZenohReply final { public: - ZenohReply(z_owned_reply_t * reply); + ZenohReply(z_owned_reply_t reply); ~ZenohReply(); const z_loaned_reply_t * get_reply() const; private: - z_owned_reply_t * reply_; + z_owned_reply_t reply_; }; ///============================================================================= From 8d3fce9378636615a647f28c3f481dde7a54b7df Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 27 Sep 2024 12:12:51 +0200 Subject: [PATCH 67/77] chore: update to zenoh-c 1.0.0.9 (#23) --- 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 f0737d1e..5fdbd77c 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -26,7 +26,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - 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 1.0.0.8 + VCS_VERSION 1.0.0.9 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" From 73d68afb0a353ebe6e50a57a8b2704e2fe6d538f Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 25 Sep 2024 13:40:08 -0700 Subject: [PATCH 68/77] Sleep for 100ms between router checks (#284) Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/rmw_init.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index aa2b495c..3763cbec 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -224,7 +224,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) 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)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } if (ret != RMW_RET_OK) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( From 16916cb02fb61ec5a94ba9b22352c519bbf80d9a Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 27 Sep 2024 06:23:39 -0700 Subject: [PATCH 69/77] Fix how total_count and total_count_change are calculated for matched events (#287) * Fix how total_count and change are calculated Signed-off-by: Yadunund * Ensure key expressions match Signed-off-by: Yadunund --------- Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/event.hpp | 4 +- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 55 +++++++++++++++--------- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 5 ++- rmw_zenoh_cpp/src/rmw_event.cpp | 2 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 11 +++++ 5 files changed, 52 insertions(+), 25 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/event.hpp b/rmw_zenoh_cpp/src/detail/event.hpp index 9710711a..f6cdb1a2 100644 --- a/rmw_zenoh_cpp/src/detail/event.hpp +++ b/rmw_zenoh_cpp/src/detail/event.hpp @@ -60,7 +60,7 @@ struct rmw_zenoh_event_status_t size_t total_count; size_t total_count_change; size_t current_count; - size_t current_count_change; + int32_t current_count_change; // The data field can be used to store serialized information for more complex statuses. std::string data; @@ -97,7 +97,7 @@ class DataCallbackManager size_t unread_count_ {0}; }; -/// Base class to be inherited by entities that support events. +/// A class to manage QoS related events. class EventsManager { public: diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 04f8fd04..71b84ab3 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -213,17 +213,20 @@ void GraphCache::handle_matched_events_for_put( for (const auto & [_, topic_data_ptr] : topic_qos_map) { if (is_pub) { // Count the number of matching subs for each set of qos settings. - if (!topic_data_ptr->subs_.empty()) { - match_count_for_entity += topic_data_ptr->subs_.size(); - } + match_count_for_entity += topic_data_ptr->subs_.size(); // Also iterate through the subs to check if any are local and if update event counters. for (liveliness::ConstEntityPtr sub_entity : topic_data_ptr->subs_) { - update_event_counters( - topic_info.name_, - ZENOH_EVENT_SUBSCRIPTION_MATCHED, - static_cast(1)); - if (is_entity_local(*sub_entity)) { - local_entities_with_events[sub_entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); + // Update counters only if key expressions match. + if (entity->topic_info()->topic_keyexpr_ == + sub_entity->topic_info().value().topic_keyexpr_) + { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_SUBSCRIPTION_MATCHED, + static_cast(1)); + if (is_entity_local(*sub_entity)) { + local_entities_with_events[sub_entity].insert(ZENOH_EVENT_SUBSCRIPTION_MATCHED); + } } } // Update event counters for the new entity-> @@ -237,17 +240,20 @@ void GraphCache::handle_matched_events_for_put( } else { // Entity is a sub. // Count the number of matching pubs for each set of qos settings. - if (!topic_data_ptr->pubs_.empty()) { - match_count_for_entity += topic_data_ptr->pubs_.size(); - } + match_count_for_entity += topic_data_ptr->pubs_.size(); // Also iterate through the pubs to check if any are local and if update event counters. for (liveliness::ConstEntityPtr pub_entity : topic_data_ptr->pubs_) { - update_event_counters( - topic_info.name_, - ZENOH_EVENT_PUBLICATION_MATCHED, - static_cast(1)); - if (is_entity_local(*pub_entity)) { - local_entities_with_events[pub_entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); + // Update counters only if key expressions match. + if (entity->topic_info()->topic_keyexpr_ == + pub_entity->topic_info().value().topic_keyexpr_) + { + update_event_counters( + topic_info.name_, + ZENOH_EVENT_PUBLICATION_MATCHED, + static_cast(1)); + if (is_entity_local(*pub_entity)) { + local_entities_with_events[pub_entity].insert(ZENOH_EVENT_PUBLICATION_MATCHED); + } } } // Update event counters for the new entity-> @@ -307,7 +313,7 @@ void GraphCache::handle_matched_events_for_del( } ///============================================================================= -void GraphCache::take_entities_with_events(EntityEventMap & entities_with_events) +void GraphCache::take_entities_with_events(const EntityEventMap & entities_with_events) { for (const auto & [local_entity, event_set] : entities_with_events) { // Trigger callback set for this entity for the event type. @@ -1262,6 +1268,13 @@ void GraphCache::set_qos_event_callback( event_cb_it->second[event_type] = std::move(callback); } +///============================================================================= +void GraphCache::remove_qos_event_callbacks(liveliness::ConstEntityPtr entity) +{ + std::lock_guard lock(graph_mutex_); + event_callbacks_.erase(entity); +} + ///============================================================================= bool GraphCache::is_entity_local(const liveliness::Entity & entity) const { @@ -1302,8 +1315,8 @@ void GraphCache::update_event_counters( } rmw_zenoh_event_status_t & status_to_update = event_statuses_[topic_name][event_id]; - status_to_update.total_count += std::abs(change); - status_to_update.total_count_change += std::abs(change); + status_to_update.total_count += std::max(0, change); + status_to_update.total_count_change += std::max(0, change); status_to_update.current_count += change; status_to_update.current_count_change = change; } diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index bd380402..58ab6dd9 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -185,6 +185,9 @@ class GraphCache final const rmw_zenoh_event_type_t & event_type, GraphCacheEventCallback callback); + /// Remove all qos event callbacks for an entity. + void remove_qos_event_callbacks(liveliness::ConstEntityPtr entity); + /// Returns true if the entity is a publisher or client. False otherwise. static bool is_entity_pub(const liveliness::Entity & entity); @@ -246,7 +249,7 @@ class GraphCache final using EntityEventMap = std::unordered_map>; - void take_entities_with_events(EntityEventMap & entities_with_events); + void take_entities_with_events(const EntityEventMap & entities_with_events); std::string zid_str_; /* diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index deec2e4f..3cb2b14a 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -158,7 +158,7 @@ rmw_event_set_callback( return RMW_RET_ERROR; } - // Both rmw_subscription_data_t and rmw_publisher_data_t inherit EventsBase. + // Both rmw_subscription_data_t and rmw_publisher_data_t store an EventsManager object. rmw_zenoh_cpp::EventsManager * event_data = static_cast(rmw_event->data); RMW_CHECK_ARGUMENT_FOR_NULL(event_data, RMW_RET_INVALID_ARGUMENT); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 06355e34..2527b47f 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -689,6 +689,10 @@ 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(node->context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); + rmw_context_impl_s * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, 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( @@ -718,6 +722,10 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) RMW_SET_ERROR_MSG("failed to undeclare pub"); ret = RMW_RET_ERROR; } + + // Remove any event callbacks registered to this publisher. + context_impl->graph_cache->remove_qos_event_callbacks(publisher_data->entity); + RMW_TRY_DESTRUCTOR(publisher_data->~rmw_publisher_data_t(), rmw_publisher_data_t, ); allocator->deallocate(publisher_data, allocator->state); } @@ -1584,6 +1592,9 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) context_impl->graph_cache->remove_querying_subscriber_callback(sub_data); } + // Remove any event callbacks registered to this subscription. + context_impl->graph_cache->remove_qos_event_callbacks(sub_data->entity); + RMW_TRY_DESTRUCTOR(sub_data->~rmw_subscription_data_t(), rmw_subscription_data_t, ); allocator->deallocate(sub_data, allocator->state); } From f08998280c9222659f61d3b08267d97cda7e604b Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 27 Sep 2024 10:50:50 -0700 Subject: [PATCH 70/77] Thread-safe access to graph cache (#258) * Make rmw_context_impl_s an RAII class Signed-off-by: Yadunund * fix regression with graph_guard_condition not triggering when entity is removed Signed-off-by: Yadunund * Have the context create the zenoh artifacts Signed-off-by: Yadunund * Add comment for session() api Signed-off-by: Yadunund * Style Signed-off-by: Yadunund * Add api to register querying_sub cb in rmw_context_impl_s Signed-off-by: Yadunund * Have rmw_context_impl_s return a shared_ptr to GraphCache Signed-off-by: Yadunund * Add todo on thread safety Signed-off-by: Yadunund * Update rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp Co-authored-by: Chris Lalancette Signed-off-by: Yadu * Address feedback Signed-off-by: Yadunund * Do not use allocator for creating graph_guard_condition Signed-off-by: Yadunund * Address feedback Signed-off-by: Yadunund --------- Signed-off-by: Yadunund Signed-off-by: Yadu Co-authored-by: Chris Lalancette --- rmw_zenoh_cpp/CMakeLists.txt | 1 + .../src/detail/rmw_context_impl_s.cpp | 383 ++++++++++++++++++ .../src/detail/rmw_context_impl_s.hpp | 138 +++++++ rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 8 - rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 32 -- rmw_zenoh_cpp/src/rmw_event.cpp | 9 +- .../src/rmw_get_node_info_and_types.cpp | 18 +- .../src/rmw_get_service_names_and_types.cpp | 6 +- .../src/rmw_get_topic_endpoint_info.cpp | 10 +- .../src/rmw_get_topic_names_and_types.cpp | 6 +- rmw_zenoh_cpp/src/rmw_init.cpp | 322 +-------------- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 68 ++-- 12 files changed, 611 insertions(+), 390 deletions(-) create mode 100644 rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp create mode 100644 rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index a8674797..14390c8c 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -34,6 +34,7 @@ add_library(rmw_zenoh_cpp SHARED src/detail/logging.cpp src/detail/message_type_support.cpp src/detail/qos.cpp + src/detail/rmw_context_impl_s.cpp src/detail/rmw_data_types.cpp src/detail/service_type_support.cpp src/detail/type_support.cpp diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp new file mode 100644 index 00000000..d60bece8 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -0,0 +1,383 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_context_impl_s.hpp" + +#include +#include +#include +#include +#include + +#include "guard_condition.hpp" +#include "identifier.hpp" +#include "liveliness_utils.hpp" +#include "logging_macros.hpp" +#include "zenoh_config.hpp" +#include "zenoh_router_check.hpp" + +#include "rcpputils/scope_exit.hpp" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" + +// Megabytes of SHM to reserve. +// TODO(clalancette): Make this configurable, or get it from the configuration +#define SHM_BUFFER_SIZE_MB 10 + +///============================================================================= +void rmw_context_impl_s::graph_sub_data_handler(const z_sample_t * sample, void * 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)); + }); + + auto data_ptr = static_cast(data); + if (data_ptr == nullptr) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "[graph_sub_data_handler] Invalid data_ptr." + ); + return; + } + + // Update the graph cache. + std::lock_guard lock(data_ptr->mutex_); + if (data_ptr->is_shutdown_) { + return; + } + switch (sample->kind) { + case z_sample_kind_t::Z_SAMPLE_KIND_PUT: + data_ptr->graph_cache_->parse_put(keystr._cstr); + break; + case z_sample_kind_t::Z_SAMPLE_KIND_DELETE: + data_ptr->graph_cache_->parse_del(keystr._cstr); + break; + default: + return; + } + + // Trigger the ROS graph guard condition. + rmw_ret_t rmw_ret = rmw_trigger_guard_condition(data_ptr->graph_guard_condition_.get()); + if (RMW_RET_OK != rmw_ret) { + RMW_ZENOH_LOG_WARN_NAMED( + "rmw_zenoh_cpp", + "[graph_sub_data_handler] Unable to trigger graph guard condition." + ); + } +} + +///============================================================================= +rmw_context_impl_s::Data::Data( + const std::string & enclave, + z_owned_session_t session, + std::optional shm_manager, + const std::string & liveliness_str, + std::shared_ptr graph_cache) +: enclave_(std::move(enclave)), + session_(std::move(session)), + shm_manager_(std::move(shm_manager)), + liveliness_str_(std::move(liveliness_str)), + graph_cache_(std::move(graph_cache)), + is_shutdown_(false), + next_entity_id_(0), + is_initialized_(false) +{ + graph_guard_condition_ = std::make_unique(); + graph_guard_condition_->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; + graph_guard_condition_->data = &guard_condition_data_; +} + +///============================================================================= +rmw_ret_t rmw_context_impl_s::Data::subscribe_to_ros_graph() +{ + std::lock_guard lock(mutex_); + if (is_initialized_) { + return RMW_RET_OK; + } + // Setup the liveliness subscriber to receives updates from the ROS graph + // and update the graph cache. + // TODO(Yadunund): This closure is still not 100% thread safe as we are + // passing Data* as the type erased argument to z_closure. Thus during + // the execution of graph_sub_data_handler, the rawptr may be freed/reset + // by a different thread. When we switch to zenoh-cpp we can replace z_closure + // with a lambda that captures a weak_ptr by copy. The lambda and caputed + // weak_ptr will have the same lifetime as the subscriber. Then within + // graph_sub_data_handler, we would first lock to weak_ptr to check if the + // shared_ptr exits. If it does, then even if a different thread calls + // rmw_context_fini() to destroy rmw_context_impl_s, the locked + // shared_ptr would live on until the graph_sub_data_handler callback. + auto sub_options = zc_liveliness_subscriber_options_null(); + z_owned_closure_sample_t callback = z_closure( + rmw_context_impl_s::graph_sub_data_handler, nullptr, this); + graph_subscriber_ = zc_liveliness_declare_subscriber( + z_loan(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( + [this]() { + z_undeclare_subscriber(z_move(this->graph_subscriber_)); + }); + if (!z_check(graph_subscriber_)) { + RMW_SET_ERROR_MSG("unable to create zenoh subscription"); + return RMW_RET_ERROR; + } + + undeclare_z_sub.cancel(); + is_initialized_ = true; + return RMW_RET_OK; +} + +///============================================================================= +rmw_ret_t rmw_context_impl_s::Data::shutdown() +{ + std::lock_guard lock(mutex_); + if (is_shutdown_) { + return RMW_RET_OK; + } + + z_undeclare_subscriber(z_move(graph_subscriber_)); + if (shm_manager_.has_value()) { + z_drop(z_move(shm_manager_.value())); + } + // Close the zenoh session + if (z_close(z_move(session_)) < 0) { + RMW_SET_ERROR_MSG("Error while closing zenoh session"); + return RMW_RET_ERROR; + } + is_shutdown_ = true; + return RMW_RET_OK; +} + +///============================================================================= +rmw_context_impl_s::Data::~Data() +{ + auto ret = this->shutdown(); + static_cast(ret); +} + +///============================================================================= +rmw_context_impl_s::rmw_context_impl_s( + const std::size_t domain_id, + const std::string & enclave) +{ + // Initialize the zenoh configuration. + z_owned_config_t config; + rmw_ret_t ret; + if ((ret = + rmw_zenoh_cpp::get_z_config( + rmw_zenoh_cpp::ConfigurableEntity::Session, + &config)) != RMW_RET_OK) + { + throw std::runtime_error("Error configuring Zenoh session."); + } + + // Check if shm is enabled. + z_owned_str_t shm_enabled = zc_config_get(z_loan(config), "transport/shared_memory/enabled"); + auto always_free_shm_enabled = rcpputils::make_scope_exit( + [&shm_enabled]() { + z_drop(z_move(shm_enabled)); + }); + + // Initialize the zenoh session. + z_owned_session_t session = z_open(z_move(config)); + if (!z_session_check(&session)) { + throw std::runtime_error("Error setting up zenoh session."); + } + auto close_session = rcpputils::make_scope_exit( + [&session]() { + z_close(z_move(session)); + }); + + // TODO(Yadunund) Move this check into a separate thread. + // Verify if the zenoh router is running if configured. + const std::optional configured_connection_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(session))) != RMW_RET_OK) { + ++connection_attempts; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + if (ret != RMW_RET_OK) { + throw std::runtime_error( + "Unable to connect to a Zenoh router after " + + std::to_string(configured_connection_attempts.value()) + + " retries."); + } + } + + // Initialize the graph cache. + const z_id_t zid = z_info_zid(z_loan(session)); + auto graph_cache = std::make_shared(zid); + // Setup liveliness subscriptions for discovery. + std::string liveliness_str = rmw_zenoh_cpp::liveliness::subscription_token( + domain_id); + + // 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(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); + // Ignore tokens from the same session to avoid race conditions from this + // query and the liveliness subscription. + graph_cache->parse_put(z_loan(keystr), true); + z_drop(z_move(keystr)); + } else { + RMW_ZENOH_LOG_DEBUG_NAMED( + "rmw_zenoh_cpp", "[rmw_context_impl_s] z_call received an invalid reply.\n"); + } + } + z_drop(z_move(reply)); + z_drop(z_move(channel)); + + // Initialize the shm manager if shared_memory is enabled in the config. + std::optional shm_manager = std::nullopt; + 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? + shm_manager = + zc_shm_manager_new( + z_loan(session), + idstr, + SHM_BUFFER_SIZE_MB * 1024 * 1024); + if (!shm_manager.has_value() || + !zc_shm_manager_check(&shm_manager.value())) + { + throw std::runtime_error("Unable to create shm manager."); + } + } + auto free_shm_manager = rcpputils::make_scope_exit( + [&shm_manager]() { + if (shm_manager.has_value()) { + z_drop(z_move(shm_manager.value())); + } + }); + + close_session.cancel(); + free_shm_manager.cancel(); + + data_ = std::make_shared( + std::move(enclave), + std::move(session), + std::move(shm_manager), + std::move(liveliness_str), + std::move(graph_cache)); + + ret = data_->subscribe_to_ros_graph(); + if (ret != RMW_RET_OK) { + throw std::runtime_error("Unable to subscribe to ROS Graph updates."); + } +} + +///============================================================================= +std::string rmw_context_impl_s::enclave() const +{ + std::lock_guard lock(data_->mutex_); + return data_->enclave_; +} + +///============================================================================= +z_session_t rmw_context_impl_s::session() const +{ + std::lock_guard lock(data_->mutex_); + return z_loan(data_->session_); +} + +///============================================================================= +std::optional & rmw_context_impl_s::shm_manager() +{ + std::lock_guard lock(data_->mutex_); + return data_->shm_manager_; +} + +///============================================================================= +rmw_guard_condition_t * rmw_context_impl_s::graph_guard_condition() +{ + std::lock_guard lock(data_->mutex_); + return data_->graph_guard_condition_.get(); +} + +///============================================================================= +size_t rmw_context_impl_s::get_next_entity_id() +{ + std::lock_guard lock(data_->mutex_); + return data_->next_entity_id_++; +} + +///============================================================================= +rmw_ret_t rmw_context_impl_s::shutdown() +{ + return data_->shutdown(); +} + +///============================================================================= +bool rmw_context_impl_s::is_shutdown() const +{ + std::lock_guard lock(data_->mutex_); + return data_->is_shutdown_; +} + +///============================================================================= +bool rmw_context_impl_s::session_is_valid() const +{ + std::lock_guard lock(data_->mutex_); + return z_check(data_->session_); +} + +///============================================================================= +std::shared_ptr rmw_context_impl_s::graph_cache() +{ + std::lock_guard lock(data_->mutex_); + return data_->graph_cache_; +} diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp new file mode 100644 index 00000000..3cb6f474 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp @@ -0,0 +1,138 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DETAIL__RMW_CONTEXT_IMPL_S_HPP_ +#define DETAIL__RMW_CONTEXT_IMPL_S_HPP_ + +#include + +# include +#include +#include +#include +#include + +#include "graph_cache.hpp" +#include "guard_condition.hpp" +#include "liveliness_utils.hpp" + +#include "rcutils/types.h" +#include "rmw/rmw.h" + +///============================================================================= +class rmw_context_impl_s final +{ +public: + // Constructor that internally initializees the Zenoh session and other artifacts. + // Throws an std::runtime_error if any of the initializations fail. + // The construction will block until a Zenoh router is detected. + // TODO(Yadunund): Make this a non-blocking call by checking for the Zenoh + // router in a separate thread. Instead block when creating a node if router + // check has not succeeded. + rmw_context_impl_s( + const std::size_t domain_id, + const std::string & enclave); + + // Get a copy of the enclave. + std::string enclave() const; + + // Loan the Zenoh session. + // TODO(Yadunund): Remove this API once rmw_context_impl_s is updated to + // create other Zenoh objects. + z_session_t session() const; + + // Get a reference to the shm_manager. + // Note: This is not thread-safe. + // TODO(Yadunund): Remove this API and instead include a publish() API + // that handles the shm_manager once the context manages publishers. + std::optional & shm_manager(); + + // Get the graph guard condition. + rmw_guard_condition_t * graph_guard_condition(); + + // Get a unique id for a new entity. + size_t get_next_entity_id(); + + // Shutdown the Zenoh session. + rmw_ret_t shutdown(); + + // Check if the Zenoh session is shutdown. + bool is_shutdown() const; + + // Returns true if the Zenoh session is valid. + bool session_is_valid() const; + + /// Return a shared_ptr to the GraphCache stored in this context. + std::shared_ptr graph_cache(); + +private: + // Bundle all class members into a data struct which can be passed as a + // weak ptr to various threads for thread-safe access without capturing + // "this" ptr by reference. + struct Data : public std::enable_shared_from_this + { + // Constructor. + Data( + const std::string & enclave, + z_owned_session_t session, + std::optional shm_manager, + const std::string & liveliness_str, + std::shared_ptr graph_cache); + + // Subscribe to the ROS graph. + rmw_ret_t subscribe_to_ros_graph(); + + // Shutdown the Zenoh session. + rmw_ret_t shutdown(); + + // Destructor. + ~Data(); + + // Mutex to lock when accessing members. + mutable std::mutex mutex_; + // RMW allocator. + const rcutils_allocator_t * allocator_; + // Enclave, name used to find security artifacts in a sros2 keystore. + std::string enclave_; + // An owned session. + z_owned_session_t session_; + // An optional SHM manager that is initialized of SHM is enabled in the + // zenoh session config. + std::optional shm_manager_; + // Liveliness keyexpr string to subscribe to for ROS graph changes. + std::string liveliness_str_; + // Graph cache. + std::shared_ptr graph_cache_; + // ROS graph liveliness subscriber. + z_owned_subscriber_t graph_subscriber_; + // Equivalent to rmw_dds_common::Context's guard condition. + // Guard condition that should be triggered when the graph changes. + std::unique_ptr graph_guard_condition_; + // The GuardCondition data structure. + rmw_zenoh_cpp::GuardCondition guard_condition_data_; + // Shutdown flag. + bool is_shutdown_; + // A counter to assign a local id for every entity created in this session. + size_t next_entity_id_; + // True once graph subscriber is initialized. + bool is_initialized_; + }; + + std::shared_ptr data_{nullptr}; + + static void graph_sub_data_handler(const z_sample_t * sample, void * data); +}; + + +#endif // DETAIL__RMW_CONTEXT_IMPL_S_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 92804f9c..f70bab6d 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -50,12 +48,6 @@ size_t hash_gid(const rmw_request_id_t & request_id) } } // namespace -///============================================================================= -size_t rmw_context_impl_s::get_next_entity_id() -{ - return next_entity_id_++; -} - 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 e52d56d8..9e09f39a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -42,38 +42,6 @@ /// Structs for various type erased data fields. -///============================================================================= -class rmw_context_impl_s final -{ -public: - // Enclave, name used to find security artifacts in a sros2 keystore. - char * enclave; - - // An owned session. - z_owned_session_t session; - - // An optional SHM provider that is initialized of SHM is enabled in the - // zenoh session config. - std::optional shm_provider; - - z_owned_subscriber_t graph_subscriber; - - /// Shutdown flag. - bool is_shutdown; - - // Equivalent to rmw_dds_common::Context's guard condition - /// Guard condition that should be triggered when the graph changes. - rmw_guard_condition_t * graph_guard_condition; - - std::unique_ptr graph_cache; - - size_t get_next_entity_id(); - -private: - // A counter to assign a local id for every entity created in this session. - size_t next_entity_id_{0}; -}; - namespace rmw_zenoh_cpp { ///============================================================================= diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index 3cb2b14a..8989726d 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -19,6 +19,7 @@ #include "detail/event.hpp" #include "detail/graph_cache.hpp" #include "detail/identifier.hpp" +#include "detail/rmw_context_impl_s.hpp" #include "detail/rmw_data_types.hpp" @@ -40,6 +41,8 @@ rmw_publisher_event_init( 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_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data->entity, RMW_RET_INVALID_ARGUMENT); if (publisher->implementation_identifier != rmw_zenoh_cpp::rmw_zenoh_identifier) { @@ -60,7 +63,7 @@ rmw_publisher_event_init( rmw_event->event_type = event_type; // Register the event with graph cache. - pub_data->context->impl->graph_cache->set_qos_event_callback( + context_impl->graph_cache()->set_qos_event_callback( pub_data->entity, zenoh_event_type, [pub_data, @@ -94,6 +97,8 @@ rmw_subscription_event_init( 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_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data->entity, RMW_RET_INVALID_ARGUMENT); if (subscription->implementation_identifier != rmw_zenoh_cpp::rmw_zenoh_identifier) { @@ -120,7 +125,7 @@ rmw_subscription_event_init( return RMW_RET_OK; } - sub_data->context->impl->graph_cache->set_qos_event_callback( + context_impl->graph_cache()->set_qos_event_callback( sub_data->entity, zenoh_event_type, [sub_data, diff --git a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp index c42295b3..9e838aec 100644 --- a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp @@ -14,7 +14,7 @@ #include "detail/identifier.hpp" #include "detail/liveliness_utils.hpp" -#include "detail/rmw_data_types.hpp" +#include "detail/rmw_context_impl_s.hpp" #include "rcutils/allocator.h" @@ -43,7 +43,9 @@ rmw_get_subscriber_names_and_types_by_node( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->get_entity_names_and_types_by_node( + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); + return context_impl->graph_cache()->get_entity_names_and_types_by_node( rmw_zenoh_cpp::liveliness::EntityType::Subscription, allocator, node_name, @@ -71,7 +73,9 @@ rmw_get_publisher_names_and_types_by_node( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->get_entity_names_and_types_by_node( + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); + return context_impl->graph_cache()->get_entity_names_and_types_by_node( rmw_zenoh_cpp::liveliness::EntityType::Publisher, allocator, node_name, @@ -98,7 +102,9 @@ rmw_get_service_names_and_types_by_node( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->get_entity_names_and_types_by_node( + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); + return context_impl->graph_cache()->get_entity_names_and_types_by_node( rmw_zenoh_cpp::liveliness::EntityType::Service, allocator, node_name, @@ -125,7 +131,9 @@ rmw_get_client_names_and_types_by_node( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->get_entity_names_and_types_by_node( + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); + return context_impl->graph_cache()->get_entity_names_and_types_by_node( rmw_zenoh_cpp::liveliness::EntityType::Client, allocator, node_name, diff --git a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp index 12415ebe..681b8ceb 100644 --- a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detail/rmw_data_types.hpp" +#include "detail/rmw_context_impl_s.hpp" #include "rcutils/allocator.h" @@ -35,8 +35,10 @@ rmw_get_service_names_and_types( RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_INVALID_ARGUMENT); + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->get_service_names_and_types( + return context_impl->graph_cache()->get_service_names_and_types( allocator, service_names_and_types); } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp index a4c4878d..3d7570cc 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -15,7 +15,7 @@ #include "detail/identifier.hpp" #include "detail/liveliness_utils.hpp" -#include "detail/rmw_data_types.hpp" +#include "detail/rmw_context_impl_s.hpp" #include "rcutils/allocator.h" @@ -43,7 +43,9 @@ rmw_get_publishers_info_by_topic( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->get_entities_info_by_topic( + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); + return context_impl->graph_cache()->get_entities_info_by_topic( rmw_zenoh_cpp::liveliness::EntityType::Publisher, allocator, topic_name, @@ -69,7 +71,9 @@ rmw_get_subscriptions_info_by_topic( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->get_entities_info_by_topic( + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); + return context_impl->graph_cache()->get_entities_info_by_topic( rmw_zenoh_cpp::liveliness::EntityType::Subscription, allocator, topic_name, diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp index db7d972c..9a3fb6c2 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detail/rmw_data_types.hpp" +#include "detail/rmw_context_impl_s.hpp" #include "rcutils/allocator.h" @@ -37,7 +37,9 @@ rmw_get_topic_names_and_types( RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->get_topic_names_and_types( + rmw_context_impl_t * context_impl = static_cast(node->context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); + return context_impl->graph_cache()->get_topic_names_and_types( allocator, no_demangle, topic_names_and_types); } } // extern "C" diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 3763cbec..7d26c36f 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -22,9 +22,9 @@ #include "detail/guard_condition.hpp" #include "detail/identifier.hpp" #include "detail/liveliness_utils.hpp" +#include "detail/rmw_context_impl_s.hpp" #include "detail/rmw_data_types.hpp" #include "detail/zenoh_config.hpp" -#include "detail/zenoh_router_check.hpp" #include "rcutils/env.h" #include "detail/logging_macros.hpp" @@ -39,54 +39,6 @@ extern "C" { -// Megabytes of SHM to reserve. -// TODO(clalancette): Make this configurable, or get it from the configuration -#define SHM_BUFFER_SIZE_MB 10 - -namespace -{ -void -graph_sub_data_handler(z_loaned_sample_t * sample, void * data) -{ - static_cast(data); - - 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); - if (context_impl == nullptr) { - RMW_ZENOH_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "[graph_sub_data_handler] Unable to convert data into context_impl" - ); - 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); - if (RMW_RET_OK != rmw_ret) { - RMW_ZENOH_LOG_WARN_NAMED( - "rmw_zenoh_cpp", - "[graph_sub_data_handler] Unable to trigger graph guard condition" - ); - } -} -} // namespace - //============================================================================== /// Initialize the middleware with the given options, and yielding an context. rmw_ret_t @@ -123,24 +75,6 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) 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 @@ -154,20 +88,6 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) } }); - // 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); - }); - - // Initialize context's implementation - context->impl->is_shutdown = false; - // If not already defined, set the logging environment variable for Zenoh sessions // to warning level by default. // TODO(Yadunund): Switch to rcutils_get_env once it supports not overwriting values. @@ -179,215 +99,29 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // Enable the zenoh built-in logger zc_try_init_log_from_env(); - // 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) - { - RMW_SET_ERROR_MSG("Error configuring Zenoh session."); - return ret; - } - - // Check if shm is enabled. - 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), NULL) != Z_OK) { - 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), NULL); - }); - - /// Initialize the graph cache. - z_id_t zid = z_info_zid(z_loan(context->impl->session)); - context->impl->graph_cache = std::make_unique(zid); - - // Verify if the zenoh router is running if configured. - const std::optional configured_connection_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) { - ++connection_attempts; - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - 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()); - return ret; - } - } - - // 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) { - RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled"); - - // TODO(yuyuan): determine the default alignment of SHM - 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)) != Z_OK) { - 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 = - 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); + // Create the context impl. + context->impl = static_cast( + allocator->zero_allocate(1, sizeof(rmw_context_impl_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( - context->impl->graph_guard_condition->data, - "failed to allocate graph guard condition data", + context->impl, + "failed to allocate context impl", return RMW_RET_BAD_ALLOC); - auto free_guard_condition_data = rcpputils::make_scope_exit( + auto free_impl = rcpputils::make_scope_exit( [context, allocator]() { - allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); + allocator->deallocate(context->impl, allocator->state); }); RMW_TRY_PLACEMENT_NEW( - context->impl->graph_guard_condition->data, - context->impl->graph_guard_condition->data, + context->impl, + context->impl, 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_context_impl_t, + context->actual_domain_id, + std::string(options->enclave) + ); - // Setup liveliness subscriptions for discovery. - 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. - - // 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_fifo_handler_reply_t handler; - z_owned_closure_reply_t closure; - z_fifo_channel_reply_new(&closure, &handler, SIZE_MAX - 1); - - 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) == Z_OK) { - 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(str, true); - } else { - RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "[discovery] Received an error\n"); - } - z_drop(z_move(reply)); - } - - z_drop(z_move(handler)); - - // 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. - // auto sub_options = z_subscriber_options_default(); - // sub_options.reliability = Z_RELIABILITY_RELIABLE; - // context->impl->graph_subscriber = z_declare_subscriber( - // z_loan(context->impl->session), - // z_keyexpr(liveliness_str.c_str()), - // z_move(callback), - // &sub_options); - 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); - - z_view_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); - - auto undeclare_z_sub = rcpputils::make_scope_exit( - [context]() { - z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); - }); - if (zc_liveliness_declare_subscriber( - &context->impl->graph_subscriber, - z_loan(context->impl->session), z_loan(keyexpr), - 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(); - free_guard_condition_data.cancel(); - free_guard_condition.cancel(); - free_enclave.cancel(); free_options.cancel(); - impl_destructor.cancel(); free_impl.cancel(); - free_shm_provider.cancel(); restore_context.cancel(); return RMW_RET_OK; @@ -409,19 +143,10 @@ rmw_shutdown(rmw_context_t * context) 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_provider.has_value()) { - z_drop(z_move(context->impl->shm_provider.value())); - } - // Close the zenoh session - if (z_close(z_move(context->impl->session), NULL) != Z_OK) { - RMW_SET_ERROR_MSG("Error while closing zenoh session"); - return RMW_RET_ERROR; - } + rmw_context_impl_t * context_impl = static_cast(context->impl); + RMW_CHECK_ARGUMENT_FOR_NULL(context_impl, RMW_RET_INVALID_ARGUMENT); - context->impl->is_shutdown = true; - - return RMW_RET_OK; + return context_impl->shutdown(); } //============================================================================== @@ -439,24 +164,13 @@ rmw_context_fini(rmw_context_t * context) context->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!context->impl->is_shutdown) { + 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; - 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 *, ); allocator->deallocate(context->impl, allocator->state); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 2527b47f..11b63d87 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -35,6 +35,7 @@ #include "detail/logging_macros.hpp" #include "detail/message_type_support.hpp" #include "detail/qos.hpp" +#include "detail/rmw_context_impl_s.hpp" #include "detail/rmw_data_types.hpp" #include "detail/serialization_format.hpp" #include "detail/type_support_common.hpp" @@ -162,11 +163,7 @@ rmw_create_node( 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) { + if (context->impl->is_shutdown()) { RCUTILS_SET_ERROR_MSG("context has been shutdown"); return nullptr; } @@ -249,13 +246,14 @@ 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(); + z_session_t session = context->impl->session(); node_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(z_loan(context->impl->session)), + z_info_zid(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}); + context->impl->enclave()}); if (node_data->entity == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -342,7 +340,7 @@ rmw_node_get_graph_guard_condition(const rmw_node_t * node) RMW_CHECK_ARGUMENT_FOR_NULL(node->context, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, nullptr); - return node->context->impl->graph_guard_condition; + return node->context->impl->graph_guard_condition(); } //============================================================================== @@ -564,7 +562,8 @@ rmw_create_publisher( allocator->deallocate(type_hash_c_str, allocator->state); }); - const z_id_t zid = z_info_zid(z_loan(node->context->impl->session)); + z_session_t session = context_impl->session(); + const z_id_t zid = z_info_zid(session); publisher_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( zid, @@ -573,7 +572,7 @@ rmw_create_publisher( 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}, + node->context->actual_domain_id, node->namespace_, node->name, context_impl->enclave()}, rmw_zenoh_cpp::liveliness::TopicInfo{ node->context->actual_domain_id, rmw_publisher->topic_name, @@ -724,7 +723,7 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) } // Remove any event callbacks registered to this publisher. - context_impl->graph_cache->remove_qos_event_callbacks(publisher_data->entity); + context_impl->graph_cache()->remove_qos_event_callbacks(publisher_data->entity); RMW_TRY_DESTRUCTOR(publisher_data->~rmw_publisher_data_t(), rmw_publisher_data_t, ); allocator->deallocate(publisher_data, allocator->state); @@ -992,7 +991,7 @@ rmw_publisher_count_matched_subscriptions( 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( + return context_impl->graph_cache()->publisher_count_matched_subscriptions( publisher, subscription_count); } @@ -1378,16 +1377,18 @@ rmw_create_subscription( allocator->deallocate(type_hash_c_str, allocator->state); }); + z_session_t session = context_impl->session(); + // Everything above succeeded and is setup properly. Now declare a subscriber // with Zenoh; after this, callbacks may come in at any time. sub_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(z_loan(node->context->impl->session)), + z_info_zid(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}, + node->context->actual_domain_id, node->namespace_, node->name, context_impl->enclave()}, rmw_zenoh_cpp::liveliness::TopicInfo{ node->context->actual_domain_id, rmw_subscription->topic_name, @@ -1450,7 +1451,7 @@ rmw_create_subscription( // Register the querying subscriber with the graph cache to get latest // messages from publishers that were discovered after their first publication. - context_impl->graph_cache->set_querying_subscriber_callback( + context_impl->graph_cache()->set_querying_subscriber_callback( sub_data, [sub_data](const std::string & queryable_prefix) -> void { @@ -1589,11 +1590,11 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) ret = RMW_RET_ERROR; } // Also remove the registered callback from the GraphCache. - context_impl->graph_cache->remove_querying_subscriber_callback(sub_data); + context_impl->graph_cache()->remove_querying_subscriber_callback(sub_data); } // Remove any event callbacks registered to this subscription. - context_impl->graph_cache->remove_qos_event_callbacks(sub_data->entity); + context_impl->graph_cache()->remove_qos_event_callbacks(sub_data->entity); RMW_TRY_DESTRUCTOR(sub_data->~rmw_subscription_data_t(), rmw_subscription_data_t, ); allocator->deallocate(sub_data, allocator->state); @@ -1627,7 +1628,7 @@ rmw_subscription_count_matched_publishers( 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( + return context_impl->graph_cache()->subscription_count_matched_publishers( subscription, publisher_count); } @@ -1846,7 +1847,7 @@ rmw_take_sequence( 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) { + if (sub_data->context->impl->is_shutdown()) { return RMW_RET_OK; } @@ -1906,7 +1907,7 @@ __rmw_take_serialized( 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) { + if (sub_data->context->impl->is_shutdown()) { return RMW_RET_OK; } @@ -2248,14 +2249,15 @@ rmw_create_client( allocator->deallocate(type_hash_c_str, allocator->state); }); + z_session_t session = context_impl->session(); client_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(z_loan(node->context->impl->session)), + z_info_zid(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}, + node->context->actual_domain_id, node->namespace_, node->name, context_impl->enclave()}, rmw_zenoh_cpp::liveliness::TopicInfo{ node->context->actual_domain_id, rmw_client->service_name, @@ -2481,7 +2483,7 @@ rmw_send_request( z_owned_closure_reply_t callback; z_closure(&callback, rmw_zenoh_cpp::client_data_handler, NULL, client_data); z_get( - z_loan(context_impl->session), + context_impl->session(), z_loan(client_data->keyexpr), "", z_move(callback), &opts); @@ -2833,14 +2835,16 @@ rmw_create_service( allocator->deallocate(type_hash_c_str, allocator->state); }); + z_session_t session = context_impl->session(); + service_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(z_loan(node->context->impl->session)), + z_info_zid(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}, + node->context->actual_domain_id, node->namespace_, node->name, context_impl->enclave()}, rmw_zenoh_cpp::liveliness::TopicInfo{ node->context->actual_domain_id, rmw_service->service_name, @@ -3634,7 +3638,7 @@ rmw_get_node_names( 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( + return node->context->impl->graph_cache()->get_node_names( node_names, node_namespaces, nullptr, allocator); } @@ -3657,7 +3661,7 @@ rmw_get_node_names_with_enclaves( 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( + return node->context->impl->graph_cache()->get_node_names( node_names, node_namespaces, enclaves, allocator); } @@ -3688,7 +3692,7 @@ rmw_count_publishers( } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->count_publishers(topic_name, count); + return node->context->impl->graph_cache()->count_publishers(topic_name, count); } //============================================================================== @@ -3718,7 +3722,7 @@ rmw_count_subscribers( } 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); } //============================================================================== @@ -3748,7 +3752,7 @@ rmw_count_clients( } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->count_clients(service_name, count); + return node->context->impl->graph_cache()->count_clients(service_name, count); } //============================================================================== @@ -3778,7 +3782,7 @@ rmw_count_services( } RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); - return node->context->impl->graph_cache->count_services(service_name, count); + return node->context->impl->graph_cache()->count_services(service_name, count); } //============================================================================== @@ -3878,7 +3882,7 @@ rmw_service_server_is_available( return RMW_RET_INVALID_ARGUMENT; } - return node->context->impl->graph_cache->service_server_is_available( + return node->context->impl->graph_cache()->service_server_is_available( client->service_name, service_type.c_str(), is_available); } From 63530b717681b15a6b84f49adfaa0ed78fed902a Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Mon, 30 Sep 2024 17:01:04 +0800 Subject: [PATCH 71/77] fix: adapt the latest changes in rolling --- .../src/detail/rmw_context_impl_s.cpp | 153 ++++++++---------- .../src/detail/rmw_context_impl_s.hpp | 18 +-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 50 ++---- 3 files changed, 94 insertions(+), 127 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index d60bece8..a90bd151 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -20,7 +20,6 @@ #include #include -#include "guard_condition.hpp" #include "identifier.hpp" #include "liveliness_utils.hpp" #include "logging_macros.hpp" @@ -29,20 +28,16 @@ #include "rcpputils/scope_exit.hpp" #include "rmw/error_handling.h" -#include "rmw/impl/cpp/macros.hpp" // Megabytes of SHM to reserve. // TODO(clalancette): Make this configurable, or get it from the configuration #define SHM_BUFFER_SIZE_MB 10 ///============================================================================= -void rmw_context_impl_s::graph_sub_data_handler(const z_sample_t * sample, void * data) +void rmw_context_impl_s::graph_sub_data_handler(z_loaned_sample_t * sample, void * 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); auto data_ptr = static_cast(data); if (data_ptr == nullptr) { @@ -58,12 +53,13 @@ void rmw_context_impl_s::graph_sub_data_handler(const z_sample_t * sample, void if (data_ptr->is_shutdown_) { return; } - switch (sample->kind) { + 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: - data_ptr->graph_cache_->parse_put(keystr._cstr); + data_ptr->graph_cache_->parse_put(str); break; case z_sample_kind_t::Z_SAMPLE_KIND_DELETE: - data_ptr->graph_cache_->parse_del(keystr._cstr); + data_ptr->graph_cache_->parse_del(str); break; default: return; @@ -83,12 +79,12 @@ void rmw_context_impl_s::graph_sub_data_handler(const z_sample_t * sample, void rmw_context_impl_s::Data::Data( const std::string & enclave, z_owned_session_t session, - std::optional shm_manager, + std::optional shm_provider, const std::string & liveliness_str, std::shared_ptr graph_cache) : enclave_(std::move(enclave)), session_(std::move(session)), - shm_manager_(std::move(shm_manager)), + shm_provider_(std::move(shm_provider)), liveliness_str_(std::move(liveliness_str)), graph_cache_(std::move(graph_cache)), is_shutdown_(false), @@ -119,20 +115,21 @@ rmw_ret_t rmw_context_impl_s::Data::subscribe_to_ros_graph() // shared_ptr exits. If it does, then even if a different thread calls // rmw_context_fini() to destroy rmw_context_impl_s, the locked // shared_ptr would live on until the graph_sub_data_handler callback. - auto sub_options = zc_liveliness_subscriber_options_null(); - z_owned_closure_sample_t callback = z_closure( - rmw_context_impl_s::graph_sub_data_handler, nullptr, this); - graph_subscriber_ = zc_liveliness_declare_subscriber( - z_loan(session_), - z_keyexpr(liveliness_str_.c_str()), - z_move(callback), - &sub_options); - zc_liveliness_subscriber_options_drop(z_move(sub_options)); + 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, this); + z_view_keyexpr_t keyexpr; + z_view_keyexpr_from_str(&keyexpr, liveliness_str_.c_str()); auto undeclare_z_sub = rcpputils::make_scope_exit( [this]() { z_undeclare_subscriber(z_move(this->graph_subscriber_)); }); - if (!z_check(graph_subscriber_)) { + if (zc_liveliness_declare_subscriber( + &graph_subscriber_, + z_loan(session_), z_loan(keyexpr), + z_move(callback), &sub_options) != Z_OK) + { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return RMW_RET_ERROR; } @@ -151,11 +148,11 @@ rmw_ret_t rmw_context_impl_s::Data::shutdown() } z_undeclare_subscriber(z_move(graph_subscriber_)); - if (shm_manager_.has_value()) { - z_drop(z_move(shm_manager_.value())); + if (shm_provider_.has_value()) { + z_drop(z_move(shm_provider_.value())); } // Close the zenoh session - if (z_close(z_move(session_)) < 0) { + if (z_close(z_move(session_), NULL) != Z_OK) { RMW_SET_ERROR_MSG("Error while closing zenoh session"); return RMW_RET_ERROR; } @@ -187,20 +184,22 @@ rmw_context_impl_s::rmw_context_impl_s( } // Check if shm is enabled. - z_owned_str_t shm_enabled = zc_config_get(z_loan(config), "transport/shared_memory/enabled"); + z_owned_string_t shm_enabled; + zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled); auto always_free_shm_enabled = rcpputils::make_scope_exit( [&shm_enabled]() { z_drop(z_move(shm_enabled)); }); // Initialize the zenoh session. - z_owned_session_t session = z_open(z_move(config)); - if (!z_session_check(&session)) { + z_owned_session_t session; + if (z_open(&session, z_move(config), NULL) != Z_OK) { + RMW_SET_ERROR_MSG("Error setting up zenoh session"); throw std::runtime_error("Error setting up zenoh session."); } auto close_session = rcpputils::make_scope_exit( [&session]() { - z_close(z_move(session)); + z_close(z_move(session), NULL); }); // TODO(Yadunund) Move this check into a separate thread. @@ -247,70 +246,65 @@ rmw_context_impl_s::rmw_context_impl_s( // 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); + z_owned_fifo_handler_reply_t handler; + z_owned_closure_reply_t closure; + z_fifo_channel_reply_new(&closure, &handler, SIZE_MAX - 1); + + z_view_keyexpr_t keyexpr; + z_view_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); zc_liveliness_get( - z_loan(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); + z_loan(session), z_loan(keyexpr), + z_move(closure), NULL); + + z_owned_reply_t reply; + while (z_recv(z_loan(handler), &reply) == Z_OK) { + 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. - graph_cache->parse_put(z_loan(keystr), true); - z_drop(z_move(keystr)); + graph_cache->parse_put(str, true); } else { RMW_ZENOH_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", "[rmw_context_impl_s] z_call received an invalid reply.\n"); } + z_drop(z_move(reply)); } - z_drop(z_move(reply)); - z_drop(z_move(channel)); + z_drop(z_move(handler)); // Initialize the shm manager if shared_memory is enabled in the config. - std::optional shm_manager = std::nullopt; - 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? - shm_manager = - zc_shm_manager_new( - z_loan(session), - idstr, - SHM_BUFFER_SIZE_MB * 1024 * 1024); - if (!shm_manager.has_value() || - !zc_shm_manager_check(&shm_manager.value())) - { + std::optional shm_provider = std::nullopt; + if (strncmp(z_string_data(z_loan(shm_enabled)), "true", z_string_len(z_loan(shm_enabled))) == 0) { + RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled"); + + // TODO(yuyuan): determine the default alignment of SHM + 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)) != Z_OK) { + RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a SHM provider."); throw std::runtime_error("Unable to create shm manager."); } + shm_provider = provider; } - auto free_shm_manager = rcpputils::make_scope_exit( - [&shm_manager]() { - if (shm_manager.has_value()) { - z_drop(z_move(shm_manager.value())); + auto free_shm_provider = rcpputils::make_scope_exit( + [&shm_provider]() { + if (shm_provider.has_value()) { + z_drop(z_move(shm_provider.value())); } }); close_session.cancel(); - free_shm_manager.cancel(); + free_shm_provider.cancel(); data_ = std::make_shared( std::move(enclave), std::move(session), - std::move(shm_manager), + std::move(shm_provider), std::move(liveliness_str), std::move(graph_cache)); @@ -328,17 +322,17 @@ std::string rmw_context_impl_s::enclave() const } ///============================================================================= -z_session_t rmw_context_impl_s::session() const +const z_loaned_session_t * rmw_context_impl_s::session() const { std::lock_guard lock(data_->mutex_); return z_loan(data_->session_); } ///============================================================================= -std::optional & rmw_context_impl_s::shm_manager() +std::optional & rmw_context_impl_s::shm_provider() { std::lock_guard lock(data_->mutex_); - return data_->shm_manager_; + return data_->shm_provider_; } ///============================================================================= @@ -368,13 +362,6 @@ bool rmw_context_impl_s::is_shutdown() const return data_->is_shutdown_; } -///============================================================================= -bool rmw_context_impl_s::session_is_valid() const -{ - std::lock_guard lock(data_->mutex_); - return z_check(data_->session_); -} - ///============================================================================= std::shared_ptr rmw_context_impl_s::graph_cache() { diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp index 3cb6f474..1f2854a2 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp @@ -25,10 +25,6 @@ #include "graph_cache.hpp" #include "guard_condition.hpp" -#include "liveliness_utils.hpp" - -#include "rcutils/types.h" -#include "rmw/rmw.h" ///============================================================================= class rmw_context_impl_s final @@ -50,13 +46,13 @@ class rmw_context_impl_s final // Loan the Zenoh session. // TODO(Yadunund): Remove this API once rmw_context_impl_s is updated to // create other Zenoh objects. - z_session_t session() const; + const z_loaned_session_t * session() const; - // Get a reference to the shm_manager. + // Get a reference to the shm_provider. // Note: This is not thread-safe. // TODO(Yadunund): Remove this API and instead include a publish() API - // that handles the shm_manager once the context manages publishers. - std::optional & shm_manager(); + // that handles the shm_provider once the context manages publishers. + std::optional & shm_provider(); // Get the graph guard condition. rmw_guard_condition_t * graph_guard_condition(); @@ -86,7 +82,7 @@ class rmw_context_impl_s final Data( const std::string & enclave, z_owned_session_t session, - std::optional shm_manager, + std::optional shm_provider, const std::string & liveliness_str, std::shared_ptr graph_cache); @@ -109,7 +105,7 @@ class rmw_context_impl_s final z_owned_session_t session_; // An optional SHM manager that is initialized of SHM is enabled in the // zenoh session config. - std::optional shm_manager_; + std::optional shm_provider_; // Liveliness keyexpr string to subscribe to for ROS graph changes. std::string liveliness_str_; // Graph cache. @@ -131,7 +127,7 @@ class rmw_context_impl_s final std::shared_ptr data_{nullptr}; - static void graph_sub_data_handler(const z_sample_t * sample, void * data); + static void graph_sub_data_handler(z_loaned_sample_t * sample, void * data); }; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 11b63d87..90580f76 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -246,7 +246,7 @@ 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(); - z_session_t session = context->impl->session(); + const z_loaned_session_t * session = context->impl->session(); node_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(session), std::to_string(node_data->id), @@ -270,7 +270,7 @@ rmw_create_node( z_drop(z_move(node_data->token)); }); if (zc_liveliness_declare_token( - &node_data->token, z_loan(context->impl->session), z_loan(keyexpr), NULL) != Z_OK) + &node_data->token, session, z_loan(keyexpr), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -453,10 +453,6 @@ rmw_create_publisher( 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); rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -562,7 +558,7 @@ rmw_create_publisher( allocator->deallocate(type_hash_c_str, allocator->state); }); - z_session_t session = context_impl->session(); + const z_loaned_session_t * session = context_impl->session(); const z_id_t zid = z_info_zid(session); publisher_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( @@ -611,7 +607,7 @@ rmw_create_publisher( ze_owned_publication_cache_t pub_cache; if (ze_declare_publication_cache( - &pub_cache, z_loan(context_impl->session), z_loan(pub_ke), &pub_cache_opts)) + &pub_cache, session, z_loan(pub_ke), &pub_cache_opts)) { RMW_SET_ERROR_MSG("unable to create zenoh publisher cache"); return nullptr; @@ -639,7 +635,7 @@ rmw_create_publisher( } // 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(pub_ke), &opts) != Z_OK) + &publisher_data->pub, session, z_loan(pub_ke), &opts) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh publisher"); return nullptr; @@ -659,7 +655,7 @@ rmw_create_publisher( } }); if (zc_liveliness_declare_token( - &publisher_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), + &publisher_data->token, session, z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( @@ -877,10 +873,10 @@ rmw_publish( }); // Get memory from SHM buffer if available. - if (publisher_data->context->impl->shm_provider.has_value()) { + 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(); + 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}; @@ -1268,10 +1264,6 @@ rmw_create_subscription( 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); rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -1377,7 +1369,7 @@ rmw_create_subscription( allocator->deallocate(type_hash_c_str, allocator->state); }); - z_session_t session = context_impl->session(); + const z_loaned_session_t * session = context_impl->session(); // Everything above succeeded and is setup properly. Now declare a subscriber // with Zenoh; after this, callbacks may come in at any time. @@ -1442,7 +1434,7 @@ rmw_create_subscription( ze_owned_querying_subscriber_t sub; if (ze_declare_querying_subscriber( - &sub, z_loan(context_impl->session), z_loan(ke), z_move(callback), &sub_options)) + &sub, session, z_loan(ke), z_move(callback), &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr; @@ -1486,7 +1478,7 @@ rmw_create_subscription( z_owned_subscriber_t sub; if (z_declare_subscriber( - &sub, z_loan(context_impl->session), z_loan(sub_ke), z_move(callback), + &sub, session, z_loan(sub_ke), z_move(callback), &sub_options) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); @@ -1520,7 +1512,7 @@ rmw_create_subscription( } }); if (zc_liveliness_declare_token( - &sub_data->token, z_loan(context_impl->session), z_loan(liveliness_ke), NULL) != Z_OK) + &sub_data->token, session, z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -2063,10 +2055,6 @@ rmw_create_client( 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); RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); const rmw_zenoh_cpp::rmw_node_data_t * node_data = @@ -2249,7 +2237,7 @@ rmw_create_client( allocator->deallocate(type_hash_c_str, allocator->state); }); - z_session_t session = context_impl->session(); + const z_loaned_session_t * session = context_impl->session(); client_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(session), std::to_string(node_data->id), @@ -2294,7 +2282,7 @@ rmw_create_client( } }); if (zc_liveliness_declare_token( - &client_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), + &client_data->token, session, z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( @@ -2676,10 +2664,6 @@ rmw_create_service( 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); // SERVICE DATA ============================================================== rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -2835,7 +2819,7 @@ rmw_create_service( allocator->deallocate(type_hash_c_str, allocator->state); }); - z_session_t session = context_impl->session(); + const z_loaned_session_t * session = context_impl->session(); service_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(session), @@ -2873,7 +2857,7 @@ rmw_create_service( z_queryable_options_default(&qable_options); qable_options.complete = true; if (z_declare_queryable( - &service_data->qable, z_loan(context_impl->session), z_loan(service_data->keyexpr), + &service_data->qable, session, z_loan(service_data->keyexpr), z_move(callback), &qable_options) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh queryable"); @@ -2894,7 +2878,7 @@ rmw_create_service( } }); if (zc_liveliness_declare_token( - &service_data->token, z_loan(node->context->impl->session), z_loan(liveliness_ke), + &service_data->token, session, z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( From c8724d2e182c6b09212ebda806dae066f7d37e4a Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 2 Oct 2024 16:00:13 +0800 Subject: [PATCH 72/77] fix: addres the conflict --- rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp | 8 -------- rmw_zenoh_cpp/src/detail/rmw_node_data.cpp | 13 ++++++------- rmw_zenoh_cpp/src/detail/rmw_node_data.hpp | 2 +- 3 files changed, 7 insertions(+), 16 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index 4b547ce2..7f5ecb88 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -387,14 +387,6 @@ bool rmw_context_impl_s::create_node_data( return false; } - // Check that the Zenoh session is still valid. - if (!z_check(data_->session_)) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to create NodeData as Zenoh session is invalid."); - return false; - } - auto node_data = rmw_zenoh_cpp::NodeData::make( this->get_next_entity_id(), z_loan(data_->session_), diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp index d80bd275..2ba73b38 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp @@ -29,7 +29,7 @@ namespace rmw_zenoh_cpp ///============================================================================= std::shared_ptr NodeData::make( std::size_t id, - z_session_t session, + const z_loaned_session_t * session, std::size_t domain_id, const std::string & namespace_, const std::string & node_name, @@ -56,16 +56,15 @@ std::shared_ptr NodeData::make( } // Create the liveliness token. - zc_owned_liveliness_token_t token = zc_liveliness_declare_token( - session, - z_keyexpr(entity->liveliness_keyexpr().c_str()), - NULL - ); + std::string liveliness_keyexpr = entity->liveliness_keyexpr(); + z_view_keyexpr_t liveliness_ke; + z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); + zc_owned_liveliness_token_t token; auto free_token = rcpputils::make_scope_exit( [&token]() { z_drop(z_move(token)); }); - if (!z_check(token)) { + if (zc_liveliness_declare_token(&token, session, z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the node."); diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp index 8d11c34f..ae70458d 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp @@ -34,7 +34,7 @@ class NodeData final // Make a shared_ptr of NodeData. Returns nullptr if construction fails. static std::shared_ptr make( std::size_t id, - z_session_t session, + const z_loaned_session_t * session, std::size_t domain_id, const std::string & namespace_, const std::string & node_name, From d0229675fc150c25f2b8010cb2ddcf6e1b706f56 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 2 Oct 2024 17:45:00 +0800 Subject: [PATCH 73/77] chore: adapt the latest change --- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 1 - .../src/detail/rmw_context_impl_s.cpp | 2 - .../src/detail/rmw_context_impl_s.hpp | 4 - rmw_zenoh_cpp/src/detail/rmw_node_data.cpp | 2 +- rmw_zenoh_cpp/src/detail/rmw_node_data.hpp | 4 +- .../src/detail/rmw_publisher_data.cpp | 212 ++++++++---------- .../src/detail/rmw_publisher_data.hpp | 10 +- rmw_zenoh_cpp/src/detail/zenoh_utils.cpp | 51 ++--- rmw_zenoh_cpp/src/detail/zenoh_utils.hpp | 13 +- rmw_zenoh_cpp/src/rmw_init.cpp | 10 - rmw_zenoh_cpp/src/rmw_zenoh.cpp | 28 +-- 11 files changed, 120 insertions(+), 217 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index bf6d3ada..406aee5e 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include "event.hpp" #include "liveliness_utils.hpp" diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index be22ef75..74cdcd26 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -20,7 +20,6 @@ #include #include -#include "guard_condition.hpp" #include "identifier.hpp" #include "liveliness_utils.hpp" #include "logging_macros.hpp" @@ -29,7 +28,6 @@ #include "rcpputils/scope_exit.hpp" #include "rmw/error_handling.h" -#include "rmw/impl/cpp/macros.hpp" // Megabytes of SHM to reserve. // TODO(clalancette): Make this configurable, or get it from the configuration diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp index d7890ea0..b9f201af 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp @@ -26,12 +26,8 @@ #include "graph_cache.hpp" #include "guard_condition.hpp" -#include "liveliness_utils.hpp" #include "rmw_node_data.hpp" -#include "rcutils/types.h" -#include "rmw/rmw.h" - ///============================================================================= class rmw_context_impl_s final { diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp index 7a6910ed..76f7c05f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp @@ -121,7 +121,7 @@ std::size_t NodeData::id() const ///============================================================================= bool NodeData::create_pub_data( const rmw_publisher_t * const publisher, - z_session_t session, + const z_loaned_session_t * session, std::size_t id, const std::string & topic_name, const rosidl_message_type_support_t * type_support, diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp index ffa8f6fc..88f16431 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp @@ -26,8 +26,6 @@ #include "liveliness_utils.hpp" #include "rmw_publisher_data.hpp" -#include "rmw/rmw.h" - namespace rmw_zenoh_cpp { ///============================================================================= @@ -51,7 +49,7 @@ class NodeData final // Create a new PublisherData for a publisher in this node. bool create_pub_data( const rmw_publisher_t * const publisher, - z_session_t session, + const z_loaned_session_t * session, std::size_t id, const std::string & topic_name, const rosidl_message_type_support_t * type_support, diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index 9c54d2e5..5abdb313 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -16,14 +16,12 @@ #include -#include #include #include #include #include #include "cdr.hpp" -#include "rmw_context_impl_s.hpp" #include "message_type_support.hpp" #include "logging_macros.hpp" #include "qos.hpp" @@ -33,13 +31,15 @@ #include "rmw/error_handling.h" #include "rmw/get_topic_endpoint_info.h" -#include "rmw/impl/cpp/macros.hpp" namespace rmw_zenoh_cpp { +// TODO(yuyuan): SHM, make this configurable +#define SHM_BUF_OK_SIZE 2621440 + ///============================================================================= std::shared_ptr PublisherData::make( - z_session_t session, + const z_loaned_session_t * session, const rmw_node_t * const node, liveliness::NodeInfo node_info, std::size_t node_id, @@ -102,20 +102,18 @@ std::shared_ptr PublisherData::make( topic_name.c_str()); return nullptr; } - z_owned_keyexpr_t keyexpr = z_keyexpr_new( - pub_data->entity_->topic_info()->topic_keyexpr_.c_str()); - auto always_free_ros_keyexpr = rcpputils::make_scope_exit( - [&keyexpr]() { - z_keyexpr_drop(z_move(keyexpr)); - }); - if (!z_keyexpr_check(&keyexpr)) { + + std::string topic_keyexpr = pub_data->entity_->topic_info()->topic_keyexpr_; + z_view_keyexpr_t pub_ke; + if (z_view_keyexpr_from_str(&pub_ke, topic_keyexpr.c_str()) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh keyexpr."); return nullptr; } // Create a Publication Cache if durability is transient_local. if (adapted_qos_profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { - ze_publication_cache_options_t pub_cache_opts = ze_publication_cache_options_default(); + ze_publication_cache_options_t pub_cache_opts; + ze_publication_cache_options_default(&pub_cache_opts); pub_cache_opts.history = adapted_qos_profile.depth; pub_cache_opts.queryable_complete = true; // Set the queryable_prefix to the session id so that querying subscribers can specify this @@ -124,21 +122,19 @@ std::shared_ptr PublisherData::make( // When such a prefix is added to the PublicationCache, it listens to queries with this extra // prefix (allowing to be queried in a unique way), but still replies with the original // publications' key expressions. - z_owned_keyexpr_t queryable_prefix = z_keyexpr_new(pub_data->entity_->zid().c_str()); - auto always_free_queryable_prefix = rcpputils::make_scope_exit( - [&queryable_prefix]() { - z_keyexpr_drop(z_move(queryable_prefix)); - }); - pub_cache_opts.queryable_prefix = z_loan(queryable_prefix); - pub_data->pub_cache_ = ze_declare_publication_cache( - session, - z_loan(keyexpr), - &pub_cache_opts - ); - if (!pub_data->pub_cache_.has_value() || !z_check(pub_data->pub_cache_.value())) { + std::string queryable_prefix = pub_data->entity_->zid(); + z_view_keyexpr_t prefix_ke; + z_view_keyexpr_from_str(&prefix_ke, queryable_prefix.c_str()); + pub_cache_opts.queryable_prefix = z_loan(prefix_ke); + + ze_owned_publication_cache_t pub_cache; + if (ze_declare_publication_cache( + &pub_cache, session, z_loan(pub_ke), &pub_cache_opts)) + { RMW_SET_ERROR_MSG("unable to create zenoh publisher cache"); return nullptr; } + pub_data->pub_cache_ = pub_cache; } auto undeclare_z_publisher_cache = rcpputils::make_scope_exit( [pub_data]() { @@ -148,40 +144,42 @@ std::shared_ptr PublisherData::make( }); // 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 (adapted_qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL && - adapted_qos_profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) - { - opts.congestion_control = Z_CONGESTION_CONTROL_BLOCK; + + if (adapted_qos_profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { + opts.reliability = Z_RELIABILITY_RELIABLE; + + if (adapted_qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) { + opts.congestion_control = Z_CONGESTION_CONTROL_BLOCK; + } } // TODO(clalancette): What happens if the key name is a valid but empty string? - pub_data->pub_ = z_declare_publisher( - session, - z_loan(keyexpr), - &opts - ); - if (!z_check(pub_data->pub_)) { - RMW_SET_ERROR_MSG("Unable to create Zenoh publisher."); - return nullptr; - } auto undeclare_z_publisher = rcpputils::make_scope_exit( [pub_data]() { z_undeclare_publisher(z_move(pub_data->pub_)); }); + if (z_declare_publisher( + &pub_data->pub_, session, z_loan(pub_ke), &opts) != Z_OK) + { + RMW_SET_ERROR_MSG("Unable to create Zenoh publisher."); + return nullptr; + } - pub_data->token_ = zc_liveliness_declare_token( - session, - z_keyexpr(pub_data->entity_->liveliness_keyexpr().c_str()), - NULL - ); + std::string liveliness_keyexpr = pub_data->entity_->liveliness_keyexpr(); + z_view_keyexpr_t liveliness_ke; + z_view_keyexpr_from_str(&liveliness_ke, liveliness_keyexpr.c_str()); auto free_token = rcpputils::make_scope_exit( [pub_data]() { if (pub_data != nullptr) { z_drop(z_move(pub_data->token_)); } }); - if (!z_check(pub_data->token_)) { + if (zc_liveliness_declare_token( + &pub_data->token_, session, z_loan(liveliness_ke), + NULL) != Z_OK) + { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the publisher."); @@ -216,7 +214,7 @@ rmw_qos_profile_t PublisherData::adapted_qos_profile() const ///============================================================================= rmw_ret_t PublisherData::publish( const void * ros_message, - std::optional & shm_manager) + std::optional & shm_provider) { std::lock_guard lock(mutex_); if (is_shutdown_) { @@ -231,13 +229,13 @@ rmw_ret_t PublisherData::publish( // To store serialized message byte array. char * msg_bytes = nullptr; - std::optional shmbuf = std::nullopt; + std::optional shmbuf = std::nullopt; auto always_free_shmbuf = rcpputils::make_scope_exit( [&shmbuf]() { if (shmbuf.has_value()) { - zc_shmbuf_drop(&shmbuf.value()); + z_drop(z_move(shmbuf.value())); } - }); + }); rcutils_allocator_t * allocator = &rmw_node_->context->options.allocator; @@ -249,22 +247,23 @@ rmw_ret_t PublisherData::publish( }); // Get memory from SHM buffer if available. - if (shm_manager.has_value() && - zc_shm_manager_check(&shm_manager.value())) - { - shmbuf = zc_shm_alloc( - &shm_manager.value(), - max_data_length); - if (!z_check(shmbuf.value())) { - zc_shm_gc(&shm_manager.value()); - shmbuf = zc_shm_alloc(&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; - } + if (shm_provider.has_value()) { + RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled."); + + auto provider = 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 { + // 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)); @@ -288,46 +287,32 @@ rmw_ret_t PublisherData::publish( const size_t data_length = ser.get_serialized_data_length(); - z_owned_bytes_map_t map = - create_map_and_set_sequence_num( - sequence_number_++, - [this](z_owned_bytes_map_t * map, const char * key) - { - // Mutex already locked. - z_bytes_t gid_bytes; - gid_bytes.len = RMW_GID_STORAGE_SIZE; - gid_bytes.start = gid_; - z_bytes_map_insert_by_copy(map, z_bytes_new(key), gid_bytes); + z_owned_bytes_t attachment; + auto free_attachment = rcpputils::make_scope_exit( + [&attachment]() { + z_drop(z_move(attachment)); }); - if (!z_check(map)) { + if (!create_map_and_set_sequence_num(&attachment, sequence_number_++, gid_)) { // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } - auto always_free_attachment_map = rcpputils::make_scope_exit( - [&map]() { - z_bytes_map_drop(z_move(map)); - }); - 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); + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + free_attachment.cancel(); + options.attachment = z_move(attachment); + z_owned_bytes_t payload; 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(pub_), z_move(payload), &options); + z_bytes_serialize_from_shm_mut(&payload, z_move(shmbuf.value())); } else { - // Returns 0 if success. - ret = z_publisher_put( - z_loan(pub_), - reinterpret_cast(msg_bytes), - data_length, - &options); + z_bytes_serialize_from_buf(&payload, reinterpret_cast(msg_bytes), data_length); } - if (ret) { + + if (z_publisher_put(z_loan(pub_), z_move(payload), &options) != Z_OK) { RMW_SET_ERROR_MSG("unable to publish message"); return RMW_RET_ERROR; } @@ -338,7 +323,7 @@ rmw_ret_t PublisherData::publish( ///============================================================================= rmw_ret_t PublisherData::publish_serialized_message( const rmw_serialized_message_t * serialized_message, - std::optional & /*shm_manager*/) + std::optional & /*shm_provider*/) { eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); @@ -350,42 +335,30 @@ rmw_ret_t PublisherData::publish_serialized_message( std::lock_guard lock(mutex_); - z_owned_bytes_map_t map = rmw_zenoh_cpp::create_map_and_set_sequence_num( - sequence_number_++, - [this](z_owned_bytes_map_t * map, const char * key) - { - // Mutex already locked. - z_bytes_t gid_bytes; - gid_bytes.len = RMW_GID_STORAGE_SIZE; - gid_bytes.start = gid_; - z_bytes_map_insert_by_copy(map, z_bytes_new(key), gid_bytes); + z_owned_bytes_t attachment; + auto free_attachment = rcpputils::make_scope_exit( + [&attachment]() { + z_drop(z_move(attachment)); }); - - if (!z_check(map)) { + if (!create_map_and_set_sequence_num(&attachment, sequence_number_++, gid_)) { // 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)); - }); 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); + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + free_attachment.cancel(); + options.attachment = z_move(attachment); - // Returns 0 if success. - int8_t ret = z_publisher_put( - z_loan(pub_), - serialized_message->buffer, - data_length, - &options); + z_owned_bytes_t payload; + z_bytes_serialize_from_buf(&payload, serialized_message->buffer, data_length); - if (ret) { + if (z_publisher_put(z_loan(pub_), z_move(payload), &options) != Z_OK) { RMW_SET_ERROR_MSG("unable to publish message"); return RMW_RET_ERROR; } @@ -414,13 +387,6 @@ void PublisherData::copy_gid(rmw_gid_t * gid) const memcpy(gid->data, gid_, RMW_GID_STORAGE_SIZE); } -///============================================================================= -bool PublisherData::liveliness_is_valid() const -{ - std::lock_guard lock(mutex_); - return zc_liveliness_token_check(&token_); -} - ///============================================================================= std::shared_ptr PublisherData::events_mgr() const { diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index 51b364cc..8d4feaa2 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -27,11 +27,7 @@ #include "event.hpp" #include "liveliness_utils.hpp" #include "message_type_support.hpp" -#include "type_support_common.hpp" -#include "rcutils/allocator.h" - -#include "rmw/rmw.h" #include "rmw/ret_types.h" namespace rmw_zenoh_cpp @@ -42,7 +38,7 @@ class PublisherData final public: // Make a shared_ptr of PublisherData. static std::shared_ptr make( - z_session_t session, + const z_loaned_session_t * session, const rmw_node_t * const node, liveliness::NodeInfo node_info, std::size_t node_id, @@ -57,12 +53,12 @@ class PublisherData final // Publish a ROS message. rmw_ret_t publish( const void * ros_message, - std::optional & shm_manager); + std::optional & shm_provider); // Publish a serialized ROS message. rmw_ret_t publish_serialized_message( const rmw_serialized_message_t * serialized_message, - std::optional & shm_manager); + std::optional & shm_provider); // Get a copy of the GUID of this PublisherData's liveliness::Entity. std::size_t guid() const; diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp index 79586033..0edee5d0 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp @@ -12,54 +12,35 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "zenoh_utils.hpp" - #include -#include -#include "rcpputils/scope_exit.hpp" +#include "zenoh_utils.hpp" +#include "attachment_helpers.hpp" +#include "logging_macros.hpp" -#include "rmw/error_handling.h" +#include "rmw/types.h" namespace rmw_zenoh_cpp { ///============================================================================= -z_owned_bytes_map_t +bool create_map_and_set_sequence_num( + z_owned_bytes_t * out_bytes, int64_t sequence_number, - GIDCopier gid_copier) + 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)); - }); - - // 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)); - 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(); + 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; } - z_bytes_map_insert_by_copy(&map, z_bytes_new("source_timestamp"), z_bytes_new(source_ts_str)); - gid_copier(&map, "source_gid"); - - free_attachment_map.cancel(); - return map; + return true; } } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp index ca0ff2a9..74687824 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp @@ -17,20 +17,15 @@ #include -#include - #include "rmw/types.h" namespace rmw_zenoh_cpp { ///============================================================================= -// A function to safely copy an entity's GID as a z_bytes_t into a -// z_owned_bytes_map_t for a given key. -using GIDCopier = std::function; -///============================================================================= -z_owned_bytes_map_t -create_map_and_set_sequence_num(int64_t sequence_number, GIDCopier gid_copier); - +bool +create_map_and_set_sequence_num( + z_owned_bytes_t * out_bytes, int64_t sequence_number, + uint8_t gid[RMW_GID_STORAGE_SIZE]); } // namespace rmw_zenoh_cpp #endif // DETAIL__ZENOH_UTILS_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index 7d26c36f..620477f5 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -14,22 +14,12 @@ #include -#include #include -#include - -#include "detail/guard_condition.hpp" #include "detail/identifier.hpp" -#include "detail/liveliness_utils.hpp" #include "detail/rmw_context_impl_s.hpp" -#include "detail/rmw_data_types.hpp" #include "detail/zenoh_config.hpp" -#include "rcutils/env.h" -#include "detail/logging_macros.hpp" -#include "rcutils/strdup.h" -#include "rcutils/types.h" #include "rmw/init.h" #include "rmw/impl/cpp/macros.hpp" diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 65fc85f7..d0eaa885 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -600,7 +600,7 @@ rmw_publish( return pub_data->publish( ros_message, - context_impl->shm_manager()); + context_impl->shm_provider()); } //============================================================================== @@ -707,7 +707,7 @@ rmw_publish_serialized_message( return publisher_data->publish_serialized_message( serialized_message, - context_impl->shm_manager()); + context_impl->shm_provider()); } //============================================================================== @@ -2075,16 +2075,9 @@ rmw_send_request( z_get_options_default(&opts); z_owned_bytes_t attachment; - if (!create_map_and_set_sequence_num(&attachment, *sequence_id, client_data->client_gid)) { - z_owned_bytes_map_t map = rmw_zenoh_cpp::create_map_and_set_sequence_num( - *sequence_id, - [client_data](z_owned_bytes_map_t * map, const char * key) - { - z_bytes_t gid_bytes; - gid_bytes.len = RMW_GID_STORAGE_SIZE; - gid_bytes.start = client_data->client_gid; - z_bytes_map_insert_by_copy(map, z_bytes_new(key), gid_bytes); - }); + if (!rmw_zenoh_cpp::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; } @@ -2775,18 +2768,9 @@ rmw_send_response( z_query_reply_options_default(&options); z_owned_bytes_t attachment; - if (!create_map_and_set_sequence_num( + if (!rmw_zenoh_cpp::create_map_and_set_sequence_num( &attachment, request_header->sequence_number, request_header->writer_guid)) { - z_owned_bytes_map_t map = rmw_zenoh_cpp::create_map_and_set_sequence_num( - request_header->sequence_number, - [request_header](z_owned_bytes_map_t * map, const char * key) - { - z_bytes_t gid_bytes; - gid_bytes.len = RMW_GID_STORAGE_SIZE; - gid_bytes.start = request_header->writer_guid; - z_bytes_map_insert_by_copy(map, z_bytes_new(key), gid_bytes); - }); // create_map_and_set_sequence_num already set the error return RMW_RET_ERROR; } From 1cfc9f61b70d440b18ea8f92b7190c3654add499 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 3 Oct 2024 01:04:55 +0800 Subject: [PATCH 74/77] refactor(api): align with latest serialization changes --- .../src/detail/attachment_helpers.cpp | 174 +++++------------- .../src/detail/attachment_helpers.hpp | 31 +--- rmw_zenoh_cpp/src/detail/rmw_data_types.cpp | 53 +----- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 12 +- .../src/detail/rmw_publisher_data.cpp | 45 ++--- .../src/detail/rmw_publisher_data.hpp | 3 - rmw_zenoh_cpp/src/detail/zenoh_utils.cpp | 12 +- rmw_zenoh_cpp/src/detail/zenoh_utils.hpp | 2 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 74 +++----- 9 files changed, 109 insertions(+), 297 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 6183b96c..54ea7dae 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -16,46 +16,17 @@ #include #include -#include +#include +#include #include #include "rmw/types.h" -#include "logging_macros.hpp" - #include "attachment_helpers.hpp" namespace rmw_zenoh_cpp { -attachement_context_t::attachement_context_t(std::unique_ptr && _data) -: data(std::move(_data)) {} - -bool create_attachment_iter(z_owned_bytes_t * kv_pair, void * context) -{ - attachement_context_t * ctx = reinterpret_cast(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_buf( - &v, ctx->data->source_gid, - RMW_GID_STORAGE_SIZE); - } else { - return false; - } - - z_bytes_from_pair(kv_pair, z_move(k), z_move(v)); - ctx->idx += 1; - return true; -} - attachement_data_t::attachement_data_t( const int64_t _sequence_number, const int64_t _source_timestamp, @@ -66,117 +37,70 @@ attachement_data_t::attachement_data_t( memcpy(source_gid, _source_gid, RMW_GID_STORAGE_SIZE); } -z_result_t attachement_data_t::serialize_to_zbytes(z_owned_bytes_t * attachment) +attachement_data_t::attachement_data_t(attachement_data_t && data) { - attachement_context_t context = - attachement_context_t(std::make_unique(*this)); - return z_bytes_from_iter( - attachment, create_attachment_iter, - reinterpret_cast(&context)); + sequence_number = std::move(data.sequence_number); + source_timestamp = std::move(data.source_timestamp); + memcpy(source_gid, data.source_gid, RMW_GID_STORAGE_SIZE); } - -bool get_attachment( - const z_loaned_bytes_t * const attachment, - const std::string & key, z_owned_bytes_t * val) +void attachement_data_t::serialize_to_zbytes(z_owned_bytes_t * attachment) { - if (attachment == NULL || z_bytes_is_empty(attachment)) { - return false; - } - - 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); + ze_owned_serializer_t serializer; + ze_serializer_empty(&serializer); + ze_serializer_serialize_str(z_loan_mut(serializer), "sequence_number"); + ze_serializer_serialize_int64(z_loan_mut(serializer), this->sequence_number); + ze_serializer_serialize_str(z_loan_mut(serializer), "source_timestamp"); + ze_serializer_serialize_int64(z_loan_mut(serializer), this->source_timestamp); + ze_serializer_serialize_str(z_loan_mut(serializer), "source_gid"); + ze_serializer_serialize_buf(z_loan_mut(serializer), this->source_gid, RMW_GID_STORAGE_SIZE); + ze_serializer_finish(z_move(serializer), attachment); +} - 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; - } +attachement_data_t::attachement_data_t(const z_loaned_bytes_t * attachment) +{ + ze_deserializer_t deserializer = ze_deserializer_from_bytes(attachment); + z_owned_string_t key; - z_drop(z_move(pair)); - z_drop(z_move(key_)); - z_drop(z_move(key_string)); + ze_deserializer_deserialize_string(&deserializer, &key); - if (found) { - break; - } else { - z_drop(z_move(*val)); - } + // Deserialize the sequence_number + if (std::string_view(z_string_data(z_loan(key)), + z_string_len(z_loan(key))) != "sequence_number") + { + throw std::runtime_error("sequence_number is not found in the attachment."); } - - if (!found) { - return false; + z_drop(z_move(key)); + if (ze_deserializer_deserialize_int64(&deserializer, &this->sequence_number)) { + throw std::runtime_error("Failed to deserialize the sequence_number."); } - if (z_bytes_is_empty(z_loan(*val))) { - return false; + // Deserialize the source_timestamp + ze_deserializer_deserialize_string(&deserializer, &key); + if (std::string_view(z_string_data(z_loan(key)), + z_string_len(z_loan(key))) != "source_timestamp") + { + throw std::runtime_error("source_timestamp is not found in the attachment"); } - - return true; -} - -bool get_gid_from_attachment( - const z_loaned_bytes_t * const attachment, - uint8_t gid[RMW_GID_STORAGE_SIZE]) -{ - if (attachment == NULL || z_bytes_is_empty(attachment)) { - return false; + z_drop(z_move(key)); + if (ze_deserializer_deserialize_int64(&deserializer, &this->source_timestamp)) { + throw std::runtime_error("Failed to deserialize the source_timestamp."); } - z_owned_bytes_t val; - if (!get_attachment(attachment, "source_gid", &val)) { - return false; + // Deserialize the source_gid + ze_deserializer_deserialize_string(&deserializer, &key); + if (std::string_view(z_string_data(z_loan(key)), z_string_len(z_loan(key))) != "source_gid") { + throw std::runtime_error("Invalid attachment: the key source_gid is not found"); } - + z_drop(z_move(key)); z_owned_slice_t slice; - z_bytes_deserialize_into_slice(z_loan(val), &slice); + if (ze_deserializer_deserialize_slice(&deserializer, &slice)) { + throw std::runtime_error("Failed to deserialize the source_gid."); + } if (z_slice_len(z_loan(slice)) != RMW_GID_STORAGE_SIZE) { - RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "GID length mismatched.") - return false; + throw std::runtime_error("The length of source_gid mismatched."); } - memcpy(gid, z_slice_data(z_loan(slice)), z_slice_len(z_loan(slice))); - - z_drop(z_move(val)); + memcpy(this->source_gid, z_slice_data(z_loan(slice)), z_slice_len(z_loan(slice))); 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 (attachment == NULL || z_bytes_is_empty(attachment)) { - return -1; - } - - // 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; - } - - int64_t num; - if (z_bytes_deserialize_into_int64(z_loan(val), &num) != Z_OK) { - return -1; - } - - if (num == 0) { - // This is an error regardless; the client should never send this - return -1; - } - - z_drop(z_move(val)); - - return num; } } // 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 c31714c1..2648b667 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -17,9 +17,6 @@ #include -#include -#include - #include "rmw/types.h" namespace rmw_zenoh_cpp @@ -28,35 +25,19 @@ 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]; explicit attachement_data_t( const int64_t _sequence_number, const int64_t _source_timestamp, const uint8_t _source_gid[RMW_GID_STORAGE_SIZE]); - z_result_t serialize_to_zbytes(z_owned_bytes_t *); -}; + explicit attachement_data_t(const z_loaned_bytes_t *); + explicit attachement_data_t(attachement_data_t && data); -class attachement_context_t final -{ -public: - std::unique_ptr data; - int length = 3; - int idx = 0; + int64_t sequence_number; + int64_t source_timestamp; + uint8_t source_gid[RMW_GID_STORAGE_SIZE]; - attachement_context_t(std::unique_ptr && _data); + void serialize_to_zbytes(z_owned_bytes_t *); }; - -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/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index ab0fa72e..2f9e24c7 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -54,12 +54,9 @@ namespace rmw_zenoh_cpp 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) + attachement_data_t && attachment_) +: payload(p), recv_timestamp(recv_ts), attachment(std::move(attachment_)) { - memcpy(publisher_gid, pub_gid, RMW_GID_STORAGE_SIZE); } ///============================================================================= @@ -146,10 +143,11 @@ void rmw_subscription_data_t::add_new_message( } // Check for messages lost if the new sequence number is not monotonically increasing. - const size_t gid_hash = hash_gid(msg->publisher_gid); + const size_t gid_hash = hash_gid(msg->attachment.source_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->attachment.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; @@ -162,7 +160,7 @@ void rmw_subscription_data_t::add_new_message( } } // Always update the last known sequence number for the publisher - last_known_published_msg_[gid_hash] = msg->sequence_number; + last_known_published_msg_[gid_hash] = msg->attachment.sequence_number; message_queue_.emplace_back(std::move(msg)); @@ -432,48 +430,17 @@ void sub_data_handler( return; } - uint8_t pub_gid[RMW_GID_STORAGE_SIZE]; - 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."); - } - - 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."); - } - - 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."); - } - + attachement_data_t attachment(z_sample_attachment(sample)); const z_loaned_bytes_t * payload = z_sample_payload(sample); z_owned_slice_t slice; - z_bytes_deserialize_into_slice(payload, &slice); + z_bytes_to_slice(payload, &slice); sub_data->add_new_message( std::make_unique( slice, z_timestamp_ntp64_time(z_sample_timestamp(sample)), - pub_gid, - sequence_number, - source_timestamp), + std::move(attachment)), z_string_data(z_loan(keystr))); } @@ -572,7 +539,7 @@ void client_data_handler(z_loaned_reply_t * reply, void * data) const z_loaned_bytes_t * err_payload = z_reply_err_payload(err); z_owned_string_t err_str; - z_bytes_deserialize_into_string(err_payload, &err_str); + z_bytes_to_string(err_payload, &err_str); RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "z_reply_is_ok returned False for keyexpr %s. Reason: %.*s", diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index e5311091..0456b260 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -39,6 +39,7 @@ #include "message_type_support.hpp" #include "rmw_wait_set_data.hpp" #include "service_type_support.hpp" +#include "attachment_helpers.hpp" /// Structs for various type erased data fields. @@ -50,20 +51,13 @@ void sub_data_handler(z_loaned_sample_t * sample, void * sub_data); struct saved_msg_data { - explicit 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); + explicit saved_msg_data(z_owned_slice_t p, uint64_t recv_ts, attachement_data_t && attachment); ~saved_msg_data(); z_owned_slice_t payload; uint64_t recv_timestamp; - uint8_t publisher_gid[RMW_GID_STORAGE_SIZE]; - int64_t sequence_number; - int64_t source_timestamp; + attachement_data_t attachment; }; ///============================================================================= diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index 5abdb313..2e44d661 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -287,34 +287,31 @@ rmw_ret_t PublisherData::publish( const size_t data_length = ser.get_serialized_data_length(); - z_owned_bytes_t attachment; - auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() { - z_drop(z_move(attachment)); - }); - if (!create_map_and_set_sequence_num(&attachment, sequence_number_++, 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); - free_attachment.cancel(); + z_owned_bytes_t attachment; + create_map_and_set_sequence_num(&attachment, sequence_number_++, gid_); 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())); + z_bytes_from_shm_mut(&payload, z_move(shmbuf.value())); } else { - z_bytes_serialize_from_buf(&payload, reinterpret_cast(msg_bytes), data_length); + z_bytes_copy_from_buf(&payload, reinterpret_cast(msg_bytes), data_length); } - if (z_publisher_put(z_loan(pub_), z_move(payload), &options) != Z_OK) { - RMW_SET_ERROR_MSG("unable to publish message"); - return RMW_RET_ERROR; + z_result_t res = z_publisher_put(z_loan(pub_), z_move(payload), &options); + if (res != Z_OK) { + if (res == Z_ESESSION_CLOSED) { + RMW_ZENOH_LOG_WARN_NAMED("rmw_zenoh_cpp", + "unable to publish message since the zenoh session is closed"); + } else { + RMW_SET_ERROR_MSG("unable to publish message"); + return RMW_RET_ERROR; + } } return RMW_RET_OK; @@ -335,16 +332,6 @@ rmw_ret_t PublisherData::publish_serialized_message( std::lock_guard lock(mutex_); - z_owned_bytes_t attachment; - auto free_attachment = rcpputils::make_scope_exit( - [&attachment]() { - z_drop(z_move(attachment)); - }); - if (!create_map_and_set_sequence_num(&attachment, sequence_number_++, gid_)) { - // create_map_and_set_sequence_num already set the error - return RMW_RET_ERROR; - } - const size_t data_length = ser.get_serialized_data_length(); // The encoding is simply forwarded and is useful when key expressions in the @@ -352,11 +339,13 @@ rmw_ret_t PublisherData::publish_serialized_message( // will be encoded with CDR so it does not really matter. z_publisher_put_options_t options; z_publisher_put_options_default(&options); - free_attachment.cancel(); + z_owned_bytes_t attachment; + create_map_and_set_sequence_num(&attachment, sequence_number_++, gid_); + options.attachment = z_move(attachment); z_owned_bytes_t payload; - z_bytes_serialize_from_buf(&payload, serialized_message->buffer, data_length); + z_bytes_copy_from_buf(&payload, serialized_message->buffer, data_length); if (z_publisher_put(z_loan(pub_), z_move(payload), &options) != Z_OK) { RMW_SET_ERROR_MSG("unable to publish message"); diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index 8d4feaa2..f6b82255 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -69,9 +69,6 @@ class PublisherData final // Copy the GID of this PublisherData into an rmw_gid_t. void copy_gid(rmw_gid_t * gid) const; - // Returns true if liveliness token is still valid. - bool liveliness_is_valid() const; - // Get the events manager of this PublisherData. std::shared_ptr events_mgr() const; diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp index 0edee5d0..3321034b 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp @@ -16,14 +16,13 @@ #include "zenoh_utils.hpp" #include "attachment_helpers.hpp" -#include "logging_macros.hpp" #include "rmw/types.h" namespace rmw_zenoh_cpp { ///============================================================================= -bool +void create_map_and_set_sequence_num( z_owned_bytes_t * out_bytes, int64_t sequence_number, @@ -34,13 +33,6 @@ create_map_and_set_sequence_num( 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; + data.serialize_to_zbytes(out_bytes); } } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp index 74687824..fba21bad 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp @@ -22,7 +22,7 @@ namespace rmw_zenoh_cpp { ///============================================================================= -bool +void create_map_and_set_sequence_num( z_owned_bytes_t * out_bytes, int64_t sequence_number, uint8_t gid[RMW_GID_STORAGE_SIZE]); diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index d0eaa885..1fc7d9d6 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -746,10 +746,6 @@ rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) auto pub_data = node_data->get_pub_data(publisher); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); - if (!pub_data->liveliness_is_valid()) { - return RMW_RET_ERROR; - } - return RMW_RET_OK; } @@ -1354,13 +1350,13 @@ __rmw_take_one( } if (message_info != nullptr) { - message_info->source_timestamp = msg_data->source_timestamp; + message_info->source_timestamp = msg_data->attachment.source_timestamp; message_info->received_timestamp = msg_data->recv_timestamp; - message_info->publication_sequence_number = msg_data->sequence_number; + message_info->publication_sequence_number = msg_data->attachment.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); + memcpy(message_info->publisher_gid.data, msg_data->attachment.source_gid, RMW_GID_STORAGE_SIZE); message_info->from_intra_process = false; } @@ -1569,13 +1565,13 @@ __rmw_take_serialized( *taken = true; if (message_info != nullptr) { - message_info->source_timestamp = msg_data->source_timestamp; + message_info->source_timestamp = msg_data->attachment.source_timestamp; message_info->received_timestamp = msg_data->recv_timestamp; - message_info->publication_sequence_number = msg_data->sequence_number; + message_info->publication_sequence_number = msg_data->attachment.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); + memcpy(message_info->publisher_gid.data, msg_data->attachment.source_gid, RMW_GID_STORAGE_SIZE); message_info->from_intra_process = false; } @@ -2075,12 +2071,8 @@ rmw_send_request( z_get_options_default(&opts); z_owned_bytes_t attachment; - if (!rmw_zenoh_cpp::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; - } + rmw_zenoh_cpp::create_map_and_set_sequence_num(&attachment, *sequence_id, + client_data->client_gid); auto free_attachment = rcpputils::make_scope_exit( [&attachment]() { z_drop(z_move(attachment)); @@ -2104,7 +2096,7 @@ rmw_send_request( opts.consolidation = z_query_consolidation_latest(); z_owned_bytes_t payload; - z_bytes_serialize_from_buf( + z_bytes_copy_from_buf( &payload, reinterpret_cast(request_bytes), data_length); opts.payload = z_move(payload); @@ -2160,7 +2152,7 @@ rmw_take_response( } z_owned_slice_t payload; - z_bytes_deserialize_into_slice(z_sample_payload(sample), &payload); + z_bytes_to_slice(z_sample_payload(sample), &payload); // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( @@ -2180,28 +2172,20 @@ rmw_take_response( // Fill in the request_header - request_header->request_id.sequence_number = - rmw_zenoh_cpp::get_int64_from_attachment(z_sample_attachment(sample), "sequence_number"); + rmw_zenoh_cpp::attachement_data_t attachment(z_sample_attachment(sample)); + + request_header->request_id.sequence_number = attachment.sequence_number; if (request_header->request_id.sequence_number < 0) { 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(z_sample_attachment(sample), "source_timestamp"); + request_header->source_timestamp = attachment.source_timestamp; if (request_header->source_timestamp < 0) { 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( - 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; - } - auto now = std::chrono::system_clock::now().time_since_epoch(); auto now_ns = std::chrono::duration_cast(now); request_header->received_timestamp = now_ns.count(); @@ -2631,7 +2615,7 @@ rmw_take_request( // DESERIALIZE MESSAGE ======================================================== z_owned_slice_t payload; - z_bytes_deserialize_into_slice(z_query_payload(loaned_query), &payload); + z_bytes_to_slice(z_query_payload(loaned_query), &payload); // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer( @@ -2651,32 +2635,20 @@ rmw_take_request( // Fill in the request header. - // Get the sequence_number out of the attachment - const z_loaned_bytes_t * attachment = z_query_attachment(loaned_query); + rmw_zenoh_cpp::attachement_data_t attachment(z_query_attachment(loaned_query)); - request_header->request_id.sequence_number = - rmw_zenoh_cpp::get_int64_from_attachment(attachment, "sequence_number"); + request_header->request_id.sequence_number = attachment.sequence_number; if (request_header->request_id.sequence_number < 0) { 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 = attachment.source_timestamp; if (request_header->source_timestamp < 0) { 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)) - { - RMW_SET_ERROR_MSG("Could not get client GID from attachment"); - return RMW_RET_ERROR; - } - auto now = std::chrono::system_clock::now().time_since_epoch(); auto now_ns = std::chrono::duration_cast(now); request_header->received_timestamp = now_ns.count(); @@ -2768,16 +2740,12 @@ rmw_send_response( z_query_reply_options_default(&options); z_owned_bytes_t attachment; - if (!rmw_zenoh_cpp::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; - } + rmw_zenoh_cpp::create_map_and_set_sequence_num(&attachment, request_header->sequence_number, + request_header->writer_guid); options.attachment = z_move(attachment); z_owned_bytes_t payload; - z_bytes_serialize_from_buf( + z_bytes_copy_from_buf( &payload, reinterpret_cast(response_bytes), data_length); z_query_reply( loaned_query, z_loan(service_data->keyexpr), z_move(payload), &options); From bb64fde48ce94f0f3b4babb365ae6dfb6a7cc615 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 3 Oct 2024 01:24:50 +0800 Subject: [PATCH 75/77] chore(deps): bump up zenoh-c to 1.0.0.10 --- 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 5fdbd77c..f7e6fea0 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -26,7 +26,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - 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 1.0.0.9 + VCS_VERSION 1.0.0.10 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" From 4aef9e7303d73d68b4157448e9746d14346731f7 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 2 Oct 2024 22:54:38 +0200 Subject: [PATCH 76/77] chore(api): align with latest serialization changes --- rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index 2e44d661..b700baf9 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -347,9 +347,15 @@ rmw_ret_t PublisherData::publish_serialized_message( z_owned_bytes_t payload; z_bytes_copy_from_buf(&payload, serialized_message->buffer, data_length); - if (z_publisher_put(z_loan(pub_), z_move(payload), &options) != Z_OK) { - RMW_SET_ERROR_MSG("unable to publish message"); - return RMW_RET_ERROR; + z_result_t res = z_publisher_put(z_loan(pub_), z_move(payload), &options); + if (res != Z_OK) { + if (res == Z_ESESSION_CLOSED) { + RMW_ZENOH_LOG_WARN_NAMED("rmw_zenoh_cpp", + "unable to publish message since the zenoh session is closed"); + } else { + RMW_SET_ERROR_MSG("unable to publish message"); + return RMW_RET_ERROR; + } } return RMW_RET_OK; From c386a3dd1678c817d8c4aa698ee312d04c787025 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 3 Oct 2024 14:27:51 +0800 Subject: [PATCH 77/77] fix: correct the sub_ke and selector_ke in the querying_subscriber --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 1fc7d9d6..d05ae297 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1050,9 +1050,9 @@ rmw_create_subscription( // '*' in place of the queryable_prefix of each PublicationCache const std::string selector = "*/" + sub_data->entity->topic_info()->topic_keyexpr_; - z_view_keyexpr_t ke; - z_view_keyexpr_from_str(&ke, selector.c_str()); - sub_options.query_selector = z_loan(ke); + z_view_keyexpr_t selector_ke; + z_view_keyexpr_from_str(&selector_ke, selector.c_str()); + sub_options.query_selector = z_loan(selector_ke); // Tell the PublicationCache's Queryable that the query accepts any key expression as a reply. // By default a query accepts only replies that matches its query selector. // This allows us to selectively query certain PublicationCaches when defining the @@ -1072,7 +1072,7 @@ rmw_create_subscription( ze_owned_querying_subscriber_t sub; if (ze_declare_querying_subscriber( - &sub, session, z_loan(ke), z_move(callback), &sub_options)) + &sub, session, z_loan(sub_ke), z_move(callback), &sub_options)) { RMW_SET_ERROR_MSG("unable to create zenoh subscription"); return nullptr;