From 4f369c5b1cf3a9ce69d2bd4e3bd53846dc2ed2fb Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Mon, 9 Sep 2024 15:36:21 +0800 Subject: [PATCH 01/22] 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 02/22] 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 03/22] 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 04/22] 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 05/22] 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 06/22] 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 07/22] 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 08/22] 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 09/22] 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 10/22] 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 11/22] 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 aa69da98c43cb0e35f7196ef25e5937bf4d7c879 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 13 Sep 2024 00:17:21 +0200 Subject: [PATCH 12/22] TODO to reenable tests on rclcpp for content filtering (#281) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * TODO to reenable tests on rclcpp for content filtering Signed-off-by: Alejandro Hernández Cordero * Update rmw_zenoh_cpp/src/rmw_zenoh.cpp Co-authored-by: Yadu Signed-off-by: Alejandro Hernández Cordero * Update rmw_zenoh_cpp/src/rmw_zenoh.cpp Co-authored-by: Yadu Signed-off-by: Alejandro Hernández Cordero --------- Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Yadu --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index d279eabc..05232a7c 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1693,6 +1693,8 @@ rmw_subscription_set_content_filter( rmw_subscription_t * subscription, const rmw_subscription_content_filter_options_t * options) { + // Re-enable test in rclcpp when this feature is implemented + // https://github.com/ros2/rclcpp/pull/2627 static_cast(subscription); static_cast(options); return RMW_RET_UNSUPPORTED; @@ -1706,6 +1708,8 @@ rmw_subscription_get_content_filter( rcutils_allocator_t * allocator, rmw_subscription_content_filter_options_t * options) { + // Re-enable test in rclcpp when this feature is implemented + // https://github.com/ros2/rclcpp/pull/2627 static_cast(subscription); static_cast(allocator); static_cast(options); From e9d05133e8f7787a516a6ae9758652e21b3185bf Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Mon, 16 Sep 2024 15:37:56 +0800 Subject: [PATCH 13/22] 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 14/22] 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 15/22] 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 16/22] 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 25e51f2729129d11c77d531cfebf893cd8d03a6b Mon Sep 17 00:00:00 2001 From: Yadu Date: Tue, 17 Sep 2024 04:28:45 -0700 Subject: [PATCH 17/22] Fix #279 by removing querying sub callback after subscription is destroyed (#282) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Yadunund Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Alejandro Hernández Cordero --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 24 +++++++++++++++++++----- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 13 +++++++++++-- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 8 +++++++- 3 files changed, 37 insertions(+), 8 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index e27acef6..dc651479 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -412,8 +412,8 @@ void GraphCache::parse_put( { auto sub_cbs_it = querying_subs_cbs_.find(entity->topic_info()->topic_keyexpr_); if (sub_cbs_it != querying_subs_cbs_.end()) { - for (const auto & cb : sub_cbs_it->second) { - cb(entity->zid()); + for (auto sub_it = sub_cbs_it->second.begin(); sub_it != sub_cbs_it->second.end(); ++sub_it) { + sub_it->second(entity->zid()); } } } @@ -1332,15 +1332,29 @@ std::unique_ptr GraphCache::take_event_status( ///============================================================================= void GraphCache::set_querying_subscriber_callback( - const std::string & keyexpr, + const rmw_subscription_data_t * sub_data, QueryingSubscriberCallback cb) { + const std::string keyexpr = sub_data->entity->topic_info()->topic_keyexpr_; auto cb_it = querying_subs_cbs_.find(keyexpr); if (cb_it == querying_subs_cbs_.end()) { - querying_subs_cbs_[keyexpr] = std::move(std::vector{}); + querying_subs_cbs_[keyexpr] = std::move( + std::unordered_map{}); cb_it = querying_subs_cbs_.find(keyexpr); } - cb_it->second.push_back(std::move(cb)); + cb_it->second.insert(std::make_pair(sub_data, std::move(cb))); +} + +///============================================================================= +void GraphCache::remove_querying_subscriber_callback( + const rmw_subscription_data_t * sub_data) +{ + auto cb_map_it = querying_subs_cbs_.find(sub_data->entity->topic_info()->topic_keyexpr_); + if (cb_map_it == querying_subs_cbs_.end()) { + return; + } + cb_map_it->second.erase(sub_data); } } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 664f2461..f98f38bf 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -38,6 +38,11 @@ namespace rmw_zenoh_cpp { +// Forward declare to prevent circular dependency. +// TODO(Yadunund): Remove this once we move rmw_subscription_data_t out of +// rmw_data_types.hpp. +class rmw_subscription_data_t; + ///============================================================================= // TODO(Yadunund): Consider changing this to an array of unordered_set where the index of the // array corresponds to the EntityType enum. This way we don't need to mix @@ -186,9 +191,12 @@ class GraphCache final static bool is_entity_pub(const liveliness::Entity & entity); void set_querying_subscriber_callback( - const std::string & keyexpr, + const rmw_subscription_data_t * sub_data, QueryingSubscriberCallback cb); + void remove_querying_subscriber_callback( + const rmw_subscription_data_t * sub_data); + private: // Helper function to convert an Entity into a GraphNode. // Note: this will update bookkeeping variables in GraphCache. @@ -288,7 +296,8 @@ class GraphCache final // EventCallbackMap for each type of event we support in rmw_zenoh_cpp. GraphEventCallbackMap event_callbacks_; // Map keyexpressions to QueryingSubscriberCallback. - std::unordered_map> querying_subs_cbs_; + std::unordered_map> querying_subs_cbs_; // Counters to track changes to event statues for each topic. std::unordered_map> event_statuses_; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 05232a7c..577cb34e 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1493,7 +1493,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( - sub_data->entity->topic_info()->topic_keyexpr_, + sub_data, [sub_data](const std::string & queryable_prefix) -> void { if (sub_data == nullptr) { @@ -1589,6 +1589,10 @@ 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(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(subscription, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, @@ -1626,6 +1630,8 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) RMW_SET_ERROR_MSG("failed to undeclare sub"); ret = RMW_RET_ERROR; } + // Also remove the registered callback from the GraphCache. + context_impl->graph_cache->remove_querying_subscriber_callback(sub_data); } RMW_TRY_DESTRUCTOR(sub_data->~rmw_subscription_data_t(), rmw_subscription_data_t, ); From 54dd96a8da63e24f971fe57220d3dd045df91136 Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Thu, 19 Sep 2024 23:17:05 +0800 Subject: [PATCH 18/22] 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 19/22] 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 20/22] 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 21/22] 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 22/22] 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...",