From f59f93a2f42402f7275dd90d84fbec71668961a3 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:26:40 +0300 Subject: [PATCH 01/27] C++ SHM --- rmw_zenoh_cpp/CMakeLists.txt | 10 ++ .../DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 | 2 +- .../DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 | 2 +- .../src/detail/rmw_context_impl_s.cpp | 44 ++++---- .../src/detail/rmw_context_impl_s.hpp | 6 +- .../src/detail/rmw_publisher_data.cpp | 106 +++++++++++++++--- .../src/detail/rmw_publisher_data.hpp | 15 ++- .../src/detail/rmw_subscription_data.hpp | 8 +- rmw_zenoh_cpp/src/detail/shm_context.cpp | 31 +++++ rmw_zenoh_cpp/src/detail/shm_context.hpp | 34 ++++++ rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 98 ++++++++++++++++ rmw_zenoh_cpp/src/detail/zenoh_config.hpp | 30 +++++ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 14 ++- zenoh_cpp_vendor/CMakeLists.txt | 12 +- 14 files changed, 360 insertions(+), 52 deletions(-) create mode 100644 rmw_zenoh_cpp/src/detail/shm_context.cpp create mode 100644 rmw_zenoh_cpp/src/detail/shm_context.hpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 732cd470..c8b36b39 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -11,6 +11,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY TRUE CACHE BOOL "Compile Zenoh RMW with Shared Memory support") + # find dependencies find_package(ament_cmake REQUIRED) @@ -43,6 +45,7 @@ add_library(rmw_zenoh_cpp SHARED src/detail/rmw_subscription_data.cpp src/detail/service_type_support.cpp src/detail/simplified_xxhash3.cpp + src/detail/shm_context.cpp src/detail/type_support.cpp src/detail/type_support_common.cpp src/detail/zenoh_config.cpp @@ -81,6 +84,13 @@ target_compile_definitions(rmw_zenoh_cpp RMW_VERSION_PATCH=${rmw_VERSION_PATCH} ) +if(${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}) + target_compile_definitions(rmw_zenoh_cpp + PUBLIC + RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + ) +endif() + ament_export_targets(export_rmw_zenoh_cpp) register_rmw_implementation( diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 index ed12a361..b78dc031 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 @@ -461,7 +461,7 @@ /// subscriber side. By doing so, the probing procedure will succeed and shared memory will operate as expected. /// /// ROS setting: disabled by default until fully tested - enabled: false, + enabled: true, }, auth: { /// The configuration of authentication. diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 index daffd790..42716f24 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 @@ -464,7 +464,7 @@ /// subscriber side. By doing so, the probing procedure will succeed and shared memory will operate as expected. /// /// ROS setting: disabled by default until fully tested - enabled: false, + enabled: true, }, auth: { /// The configuration of authentication. diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index 61dd9c8b..2f5c9fbc 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -165,21 +165,21 @@ class rmw_context_impl_s::Data final : public std::enable_shared_from_this "rmw_zenoh_cpp", "[rmw_context_impl_s] z_call received an invalid reply.\n"); } } - - // Initialize the shm manager if shared_memory is enabled in the config. - shm_provider_ = std::nullopt; -#ifndef _MSC_VER - if (shm_enabled == "true") { - auto layout = zenoh::MemoryLayout( - SHM_BUFFER_SIZE_MB * 1024 * 1024, - zenoh::AllocAlignment({5})); - zenoh::PosixShmProvider provider(layout, &result); - if (result != Z_OK) { - throw std::runtime_error("Unable to create shm provider."); - } - shm_provider_ = std::move(provider); +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + // Initialize the shm subsystem if shared_memory is enabled in the config + if (rmw_zenoh_cpp::zenoh_shm_enabled()) { + RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled"); + + shm_ = std::make_optional( + rmw_zenoh_cpp::ShmContext( + rmw_zenoh_cpp::zenoh_shm_alloc_size(), + rmw_zenoh_cpp::zenoh_shm_message_size_threshold() + )); + } else { + RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is disabled"); } #endif + graph_guard_condition_ = std::make_unique(); graph_guard_condition_->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; graph_guard_condition_->data = &guard_condition_data_; @@ -262,11 +262,13 @@ class rmw_context_impl_s::Data final : public std::enable_shared_from_this return session_; } - std::optional & shm_provider() +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + std::optional & shm() { std::lock_guard lock(mutex_); - return shm_provider_; + return shm_; } +#endif rmw_guard_condition_t * graph_guard_condition() { @@ -398,9 +400,11 @@ class rmw_context_impl_s::Data final : public std::enable_shared_from_this std::string enclave_; // An owned session. std::shared_ptr session_; - // An optional SHM manager that is initialized of SHM is enabled in the +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + // An optional SHM context that is initialized of SHM is enabled in the // zenoh session config. - std::optional shm_provider_; + std::optional shm_; +#endif // Graph cache. std::shared_ptr graph_cache_; // ROS graph liveliness subscriber. @@ -456,11 +460,13 @@ const std::shared_ptr rmw_context_impl_s::session() const return data_->session(); } +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY ///============================================================================= -std::optional & rmw_context_impl_s::shm_provider() +std::optional & rmw_context_impl_s::shm() { - return data_->shm_provider(); + return data_->shm(); } +#endif ///============================================================================= rmw_guard_condition_t * rmw_context_impl_s::graph_guard_condition() diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp index a2fdaf5e..3c3b9eb7 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp @@ -50,11 +50,13 @@ struct rmw_context_impl_s final // Loan the Zenoh session. const std::shared_ptr session() const; - // Get a reference to the shm_provider. +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + // Get a reference to the shm subsystem. // Note: This is not thread-safe. // TODO(Yadunund): Remove this API and instead include a publish() API // that handles the shm_provider once the context manages publishers. - std::optional & shm_provider(); + std::optional & shm(); +#endif // Get the graph guard condition. rmw_guard_condition_t * graph_guard_condition(); diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index f960b879..13a8ac5a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -201,8 +201,11 @@ PublisherData::PublisherData( ///============================================================================= rmw_ret_t PublisherData::publish( - const void * ros_message, - std::optional & /*shm_provider*/) + const void * ros_message +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , const std::optional & shm +#endif +) { std::lock_guard lock(mutex_); if (is_shutdown_) { @@ -218,19 +221,59 @@ rmw_ret_t PublisherData::publish( // To store serialized message byte array. char * msg_bytes = nullptr; +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + std::optional shmbuf = std::nullopt; +#endif + rcutils_allocator_t * allocator = &rmw_node_->context->options.allocator; auto always_free_msg_bytes = rcpputils::make_scope_exit( - [&msg_bytes, allocator]() { - if (msg_bytes) { + [&msg_bytes, allocator +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , &shmbuf +#endif + ]() { + if (msg_bytes +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + && !shmbuf.has_value() +#endif + ) + { allocator->deallocate(msg_bytes, allocator->state); } }); + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + // Get memory from SHM buffer if available. + if (shm.has_value() && max_data_length >= shm.value().msgsize_threshold) { + RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled."); + + auto & provider = shm.value().shm_provider; + + // TODO(yellowhatter): SHM, use alignment based on msgsize_threshold + auto alloc_result = provider.alloc_gc_defrag_blocking( + max_data_length, + zenoh::AllocAlignment({0})); + + if (std::holds_alternative(alloc_result)) { + auto && buf = std::get(std::move(alloc_result)); + msg_bytes = reinterpret_cast(buf.data()); + shmbuf = std::make_optional(std::move(buf)); + } else { + // TODO(Yadunund): Should we revert to regular allocation and not return an error? + RMW_SET_ERROR_MSG("Failed to allocate a SHM buffer, even after GCing."); + return RMW_RET_ERROR; + } + } else { +#endif // Get memory from the allocator. msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +} +#endif // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(msg_bytes, max_data_length); @@ -257,11 +300,14 @@ rmw_ret_t PublisherData::publish( options.attachment = rmw_zenoh_cpp::AttachmentData( sequence_number_++, source_timestamp, entity_->copy_gid()).serialize_to_zbytes(); - // TODO(ahcorde): shmbuf - std::vector raw_data( - reinterpret_cast(msg_bytes), - reinterpret_cast(msg_bytes) + data_length); - zenoh::Bytes payload(std::move(raw_data)); + auto payload = +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + shmbuf.has_value() ? zenoh::Bytes(std::move(*shmbuf)) : +#endif + zenoh::Bytes( + std::vector( + reinterpret_cast(msg_bytes), + reinterpret_cast(msg_bytes) + data_length)); TRACETOOLS_TRACEPOINT( rmw_publish, static_cast(rmw_publisher_), ros_message, source_timestamp); @@ -282,8 +328,11 @@ rmw_ret_t PublisherData::publish( ///============================================================================= rmw_ret_t PublisherData::publish_serialized_message( - const rmw_serialized_message_t * serialized_message, - std::optional & /*shm_provider*/) + const rmw_serialized_message_t * serialized_message +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , const std::optional & shm +#endif +) { eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); @@ -305,14 +354,39 @@ rmw_ret_t PublisherData::publish_serialized_message( options.attachment = rmw_zenoh_cpp::AttachmentData( sequence_number_++, source_timestamp, entity_->copy_gid()).serialize_to_zbytes(); - std::vector raw_data( + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + // Get memory from SHM buffer if available. + if (shm.has_value() && data_length >= shm.value().msgsize_threshold) { + RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled."); + + auto & provider = shm.value().shm_provider; + + // TODO(yellowhatter): SHM, use alignment based on msgsize_threshold + auto alloc_result = provider.alloc_gc_defrag_blocking(data_length, zenoh::AllocAlignment({0})); + + if (std::holds_alternative(alloc_result)) { + auto && buf = std::get(std::move(alloc_result)); + auto msg_bytes = reinterpret_cast(buf.data()); + memcpy(msg_bytes, serialized_message->buffer, data_length); + zenoh::Bytes payload(std::move(buf)); + pub_.put(std::move(payload), std::move(options), &result); + } else { + // TODO(Yadunund): Should we revert to regular allocation and not return an error? + RMW_SET_ERROR_MSG("Failed to allocate a SHM buffer, even after GCing."); + return RMW_RET_ERROR; + } + } else { +#endif + std::vector raw_image( serialized_message->buffer, serialized_message->buffer + data_length); - zenoh::Bytes payload(std::move(raw_data)); - - TRACETOOLS_TRACEPOINT( - rmw_publish, static_cast(rmw_publisher_), serialized_message, source_timestamp); + zenoh::Bytes payload(raw_image); pub_.put(std::move(payload), std::move(options), &result); +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +} +#endif + if (result != Z_OK) { if (result == Z_ESESSION_CLOSED) { RMW_ZENOH_LOG_WARN_NAMED( diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index 7cec5406..c27b10e1 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -28,6 +28,7 @@ #include "event.hpp" #include "liveliness_utils.hpp" #include "message_type_support.hpp" +#include "shm_context.hpp" #include "type_support_common.hpp" #include "zenoh_utils.hpp" @@ -56,13 +57,19 @@ class PublisherData final // Publish a ROS message. rmw_ret_t publish( - const void * ros_message, - std::optional & shm_provider); + const void * ros_message +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , const std::optional & shm +#endif + ); // Publish a serialized ROS message. rmw_ret_t publish_serialized_message( - const rmw_serialized_message_t * serialized_message, - std::optional & shm_provider); + const rmw_serialized_message_t * serialized_message +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , const std::optional & shm +#endif + ); // Get a copy of the keyexpr_hash of this PublisherData's liveliness::Entity. std::size_t keyexpr_hash() const; diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp index 37ab0dba..75717af5 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp @@ -33,6 +33,7 @@ #include "liveliness_utils.hpp" #include "message_type_support.hpp" #include "attachment_helpers.hpp" +#include "shm_context.hpp" #include "type_support_common.hpp" #include "zenoh_utils.hpp" @@ -75,8 +76,11 @@ class SubscriptionData final : public std::enable_shared_from_this & shm_provider); + const void * ros_message +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , const std::optional & shm +#endif + ); // Get a copy of the keyexpr_hash of this SubscriptionData's liveliness::Entity. std::size_t keyexpr_hash() const; diff --git a/rmw_zenoh_cpp/src/detail/shm_context.cpp b/rmw_zenoh_cpp/src/detail/shm_context.cpp new file mode 100644 index 00000000..f4c39c16 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/shm_context.cpp @@ -0,0 +1,31 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "shm_context.hpp" + +namespace rmw_zenoh_cpp +{ +///============================================================================= +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +ShmContext::ShmContext(size_t alloc_size, size_t msgsize_threshold) +: // Create Layout for provider's memory + // Provider's alignment will be 1 byte as we are going to make only 1-byte aligned allocations + // TODO(yellowhatter): use zenoh_shm_message_size_threshold as base for alignment + shm_provider(zenoh::PosixShmProvider(zenoh::MemoryLayout(alloc_size, zenoh::AllocAlignment {0}))), + msgsize_threshold(msgsize_threshold) +{} +#endif +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/shm_context.hpp b/rmw_zenoh_cpp/src/detail/shm_context.hpp new file mode 100644 index 00000000..1c44dc58 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/shm_context.hpp @@ -0,0 +1,34 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DETAIL__SHM_CONTEXT_HPP_ +#define DETAIL__SHM_CONTEXT_HPP_ + +#include + +namespace rmw_zenoh_cpp +{ +///============================================================================= +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +struct ShmContext +{ + zenoh::PosixShmProvider shm_provider; + size_t msgsize_threshold; + + ShmContext(size_t alloc_size, size_t msgsize_threshold); +}; +#endif +} // namespace rmw_zenoh_cpp + +#endif // DETAIL__SHM_CONTEXT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index fa100d0a..3d4b725d 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -43,6 +43,14 @@ static const std::unordered_map _get_z_config( const char * envar_name, @@ -123,4 +131,94 @@ std::optional zenoh_router_check_attempts() // If unset, use the default. return default_value; } + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +///============================================================================= +bool zenoh_shm_enabled() +{ + const char * envar_value; + + if (NULL != rcutils_get_env(zenoh_shm_enabled_envar, &envar_value)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s cannot be read. Report this bug.", + zenoh_shm_enabled_envar); + return zenoh_shm_enabled_default; + } + + if (strlen(envar_value) == strlen("true") && + strncmp(envar_value, "true", strlen(envar_value)) == 0) + { + return true; + } + + if (strlen(envar_value) == strlen("false") && + strncmp(envar_value, "false", strlen(envar_value)) == 0) + { + return false; + } + + return zenoh_shm_enabled_default; +} +///============================================================================= +size_t zenoh_shm_alloc_size() +{ + const char * envar_value; + + if (NULL != rcutils_get_env(zenoh_shm_alloc_size_envar, &envar_value)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s cannot be read. Report this bug.", + zenoh_shm_alloc_size_envar); + return zenoh_shm_alloc_size_default; + } + + // If the environment variable contains a value, handle it accordingly. + if (envar_value[0] != '\0') { + const auto read_value = std::strtoull(envar_value, nullptr, 10); + if (read_value > 0) { + if (read_value > std::numeric_limits::max()) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s: value is too large!", + zenoh_shm_alloc_size_envar); + } else { + return read_value; + } + } + } + + return zenoh_shm_alloc_size_default; +} +///============================================================================= +size_t zenoh_shm_message_size_threshold() +{ + const char * envar_value; + + if (NULL != rcutils_get_env(zenoh_shm_message_size_threshold_envar, &envar_value)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s cannot be read. Report this bug.", + zenoh_shm_message_size_threshold_envar); + return zenoh_shm_message_size_threshold_default; + } + + // If the environment variable contains a value, handle it accordingly. + if (envar_value[0] != '\0') { + const auto read_value = std::strtoull(envar_value, nullptr, 10); + if (read_value > 0) { + if (read_value > std::numeric_limits::max()) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s: value is too large!", + zenoh_shm_message_size_threshold_envar); + } else if ((read_value & (read_value - 1)) != 0) { // power of 2 check + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", "Envar %s: value must be power of 2!", + zenoh_shm_message_size_threshold_envar); + + } else { + return read_value; + } + } + } + + return zenoh_shm_message_size_threshold_default; +} +#endif } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp index 1f1e194f..7ce7d5f4 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -61,6 +61,36 @@ std::optional get_z_config(const ConfigurableEntity & entity); /// @return The number of times to try connecting to a zenoh router and /// std::nullopt if establishing a connection to a router is not required. std::optional zenoh_router_check_attempts(); + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +///============================================================================= +/// Get the enabled state of shared memory subsystem +/// based on the environment variable ZENOH_SHM_ENABLED. +/// @details The behavior is as follows: +/// - If not set or not "false", the default value of "true" is returned. +/// - Else "false" is returned. +/// @return The amount of shared memory to be pre-allocated for Zenoh SHM operation +bool zenoh_shm_enabled(); + +///============================================================================= +/// Get the amount of shared memory to be pre-allocated for Zenoh SHM operation +/// based on the environment variable ZENOH_SHM_ALLOC_SIZE. +/// @details The behavior is as follows: +/// - If not set or <= 0, the default value of 1MB is returned. +/// - Else value of environemnt variable is returned. +/// @return The amount of shared memory to be pre-allocated for Zenoh SHM operation +size_t zenoh_shm_alloc_size(); + +///============================================================================= +/// Message size threshold for implicit SHM optimization based on the environment +/// variable ZENOH_SHM_MESSAGE_SIZE_THRESHOLD. +/// Messages smaller than this threshold will not be forwarded through Zenoh SHM +/// @details The behavior is as follows: +/// - If not set or <= 0, the default value of 2KB is returned. +/// - Else value of environemnt variable is returned. +/// @return The amount of shared memory to be pre-allocated for Zenoh SHM operation +size_t zenoh_shm_message_size_threshold(); +#endif } // namespace rmw_zenoh_cpp #endif // DETAIL__ZENOH_CONFIG_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index e7342c13..f573b04f 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -614,8 +614,11 @@ rmw_publish( } return pub_data->publish( - ros_message, - context_impl->shm_provider()); + ros_message +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , context_impl->shm() +#endif + ); } //============================================================================== @@ -721,8 +724,11 @@ rmw_publish_serialized_message( RMW_CHECK_ARGUMENT_FOR_NULL(publisher_data, RMW_RET_INVALID_ARGUMENT); return publisher_data->publish_serialized_message( - serialized_message, - context_impl->shm_provider()); + serialized_message +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , context_impl->shm() +#endif + ); } //============================================================================== diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 2d3ae046..8e1d0636 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -15,13 +15,16 @@ find_package(ament_cmake_vendor_package REQUIRED) # Note: We separate the two args needed for cargo with "$" and not ";" as the # latter is a list separater in cmake and hence the string will be split into two # when expanded. -set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memory zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") +set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") + +set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY TRUE CACHE BOOL "Compile Zenoh RMW with Shared Memory support") ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git VCS_VERSION 57d5e4d31d9b38fef34d7bcad3d3e54869c4ce73 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" + "-DZENOHC_BUILD_WITH_SHARED_MEMORY:BOOL=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}" "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" ) @@ -34,8 +37,11 @@ ament_vendor(zenoh_cpp_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-cpp VCS_VERSION 964b64dc8b935a43147287199e7bb12da7b141e6 CMAKE_ARGS - -DZENOHCXX_ZENOHC=OFF -) + -DZENOHCXX_ZENOHC=ON + -DZENOHCXX_ZENOHPICO=OFF + -DZENOHC_BUILD_WITH_SHARED_MEMORY:BOOL=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY} + -DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE + ) externalproject_add_stepdependencies(zenoh_cpp_vendor configure zenoh_c_vendor) From 449ae80a94e4ba46e737ca79dac9e142808baeeb Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:26:50 +0300 Subject: [PATCH 02/27] fix default shm msg size threshold --- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 3d4b725d..dd960529 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -49,7 +49,7 @@ static const bool zenoh_shm_enabled_default = true; static const char * zenoh_shm_alloc_size_envar = "ZENOH_SHM_ALLOC_SIZE"; static const size_t zenoh_shm_alloc_size_default = 1 * 1024 * 1024; static const char * zenoh_shm_message_size_threshold_envar = "ZENOH_SHM_MESSAGE_SIZE_THRESHOLD"; -static const size_t zenoh_shm_message_size_threshold_default = 1; //2 * 1024; +static const size_t zenoh_shm_message_size_threshold_default = 2 * 1024; #endif std::optional _get_z_config( From 5a5fa3add54dc8b30110c0f31fb0b8401ca672a5 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:26:58 +0300 Subject: [PATCH 03/27] code format fix --- rmw_zenoh_cpp/src/detail/shm_context.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/shm_context.cpp b/rmw_zenoh_cpp/src/detail/shm_context.cpp index f4c39c16..865ff69d 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.cpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.cpp @@ -21,9 +21,9 @@ namespace rmw_zenoh_cpp ///============================================================================= #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY ShmContext::ShmContext(size_t alloc_size, size_t msgsize_threshold) -: // Create Layout for provider's memory - // Provider's alignment will be 1 byte as we are going to make only 1-byte aligned allocations - // TODO(yellowhatter): use zenoh_shm_message_size_threshold as base for alignment +: // Create Layout for provider's memory + // Provider's alignment will be 1 byte as we are going to make only 1-byte aligned allocations + // TODO(yellowhatter): use zenoh_shm_message_size_threshold as base for alignment shm_provider(zenoh::PosixShmProvider(zenoh::MemoryLayout(alloc_size, zenoh::AllocAlignment {0}))), msgsize_threshold(msgsize_threshold) {} From 80e2326407a63f01d5f0c3f262c1cf0d24dec671 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:27:05 +0300 Subject: [PATCH 04/27] add lost tracepoints --- rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index 13a8ac5a..10ddeb29 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -370,6 +370,10 @@ rmw_ret_t PublisherData::publish_serialized_message( auto msg_bytes = reinterpret_cast(buf.data()); memcpy(msg_bytes, serialized_message->buffer, data_length); zenoh::Bytes payload(std::move(buf)); + + TRACETOOLS_TRACEPOINT( + rmw_publish, static_cast(rmw_publisher_), serialized_message, source_timestamp); + pub_.put(std::move(payload), std::move(options), &result); } else { // TODO(Yadunund): Should we revert to regular allocation and not return an error? @@ -382,6 +386,10 @@ rmw_ret_t PublisherData::publish_serialized_message( serialized_message->buffer, serialized_message->buffer + data_length); zenoh::Bytes payload(raw_image); + + TRACETOOLS_TRACEPOINT( + rmw_publish, static_cast(rmw_publisher_), serialized_message, source_timestamp); + pub_.put(std::move(payload), std::move(options), &result); #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY } From fd9e715a7e328c929b08a351f838584e0c6158cc Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:27:10 +0300 Subject: [PATCH 05/27] code format --- rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index 10ddeb29..40dc747f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -372,7 +372,8 @@ rmw_ret_t PublisherData::publish_serialized_message( zenoh::Bytes payload(std::move(buf)); TRACETOOLS_TRACEPOINT( - rmw_publish, static_cast(rmw_publisher_), serialized_message, source_timestamp); + rmw_publish, static_cast(rmw_publisher_), serialized_message, + source_timestamp); pub_.put(std::move(payload), std::move(options), &result); } else { From 8b2284f4533c2c01d283e1e9a21419db31553f7e Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:27:15 +0300 Subject: [PATCH 06/27] remove unnecessry flags from zenoh-cpp vendor --- zenoh_cpp_vendor/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 8e1d0636..2649a709 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -39,8 +39,6 @@ ament_vendor(zenoh_cpp_vendor CMAKE_ARGS -DZENOHCXX_ZENOHC=ON -DZENOHCXX_ZENOHPICO=OFF - -DZENOHC_BUILD_WITH_SHARED_MEMORY:BOOL=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY} - -DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE ) externalproject_add_stepdependencies(zenoh_cpp_vendor configure zenoh_c_vendor) From 048e158609ca7e7d1c1c36fc0ec6a78ebacb89f7 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:27:22 +0300 Subject: [PATCH 07/27] review fixes --- rmw_zenoh_cpp/src/detail/shm_context.cpp | 8 +++++--- rmw_zenoh_cpp/src/detail/shm_context.hpp | 6 ++++-- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 2 +- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/shm_context.cpp b/rmw_zenoh_cpp/src/detail/shm_context.cpp index 865ff69d..51d8187a 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.cpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + #include #include "shm_context.hpp" @@ -19,13 +21,13 @@ namespace rmw_zenoh_cpp { ///============================================================================= -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY ShmContext::ShmContext(size_t alloc_size, size_t msgsize_threshold) : // Create Layout for provider's memory - // Provider's alignment will be 1 byte as we are going to make only 1-byte aligned allocations + // Provider's alignment will be 1 (=2^0) bytes as we are going to make only 1-byte aligned allocations // TODO(yellowhatter): use zenoh_shm_message_size_threshold as base for alignment shm_provider(zenoh::PosixShmProvider(zenoh::MemoryLayout(alloc_size, zenoh::AllocAlignment {0}))), msgsize_threshold(msgsize_threshold) {} -#endif } // namespace rmw_zenoh_cpp + +#endif diff --git a/rmw_zenoh_cpp/src/detail/shm_context.hpp b/rmw_zenoh_cpp/src/detail/shm_context.hpp index 1c44dc58..146d9143 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.hpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.hpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + #ifndef DETAIL__SHM_CONTEXT_HPP_ #define DETAIL__SHM_CONTEXT_HPP_ @@ -20,7 +22,6 @@ namespace rmw_zenoh_cpp { ///============================================================================= -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY struct ShmContext { zenoh::PosixShmProvider shm_provider; @@ -28,7 +29,8 @@ struct ShmContext ShmContext(size_t alloc_size, size_t msgsize_threshold); }; -#endif } // namespace rmw_zenoh_cpp #endif // DETAIL__SHM_CONTEXT_HPP_ + +#endif diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index dd960529..3d4b725d 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -49,7 +49,7 @@ static const bool zenoh_shm_enabled_default = true; static const char * zenoh_shm_alloc_size_envar = "ZENOH_SHM_ALLOC_SIZE"; static const size_t zenoh_shm_alloc_size_default = 1 * 1024 * 1024; static const char * zenoh_shm_message_size_threshold_envar = "ZENOH_SHM_MESSAGE_SIZE_THRESHOLD"; -static const size_t zenoh_shm_message_size_threshold_default = 2 * 1024; +static const size_t zenoh_shm_message_size_threshold_default = 1; //2 * 1024; #endif std::optional _get_z_config( From 6a20e700a03e89d51523caec9d457307fe7c00ad Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:27:31 +0300 Subject: [PATCH 08/27] codestyle fixes --- rmw_zenoh_cpp/src/detail/shm_context.cpp | 2 +- rmw_zenoh_cpp/src/detail/shm_context.hpp | 8 ++++---- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/shm_context.cpp b/rmw_zenoh_cpp/src/detail/shm_context.cpp index 51d8187a..5078d4a5 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.cpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.cpp @@ -23,7 +23,7 @@ namespace rmw_zenoh_cpp ///============================================================================= ShmContext::ShmContext(size_t alloc_size, size_t msgsize_threshold) : // Create Layout for provider's memory - // Provider's alignment will be 1 (=2^0) bytes as we are going to make only 1-byte aligned allocations + // Provider's alignment is 1 (=2^0) bytes as we are going to make only 1-byte aligned allocations // TODO(yellowhatter): use zenoh_shm_message_size_threshold as base for alignment shm_provider(zenoh::PosixShmProvider(zenoh::MemoryLayout(alloc_size, zenoh::AllocAlignment {0}))), msgsize_threshold(msgsize_threshold) diff --git a/rmw_zenoh_cpp/src/detail/shm_context.hpp b/rmw_zenoh_cpp/src/detail/shm_context.hpp index 146d9143..450e2dea 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.hpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - #ifndef DETAIL__SHM_CONTEXT_HPP_ #define DETAIL__SHM_CONTEXT_HPP_ +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + #include namespace rmw_zenoh_cpp @@ -31,6 +31,6 @@ struct ShmContext }; } // namespace rmw_zenoh_cpp -#endif // DETAIL__SHM_CONTEXT_HPP_ - #endif + +#endif // DETAIL__SHM_CONTEXT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 3d4b725d..dd960529 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -49,7 +49,7 @@ static const bool zenoh_shm_enabled_default = true; static const char * zenoh_shm_alloc_size_envar = "ZENOH_SHM_ALLOC_SIZE"; static const size_t zenoh_shm_alloc_size_default = 1 * 1024 * 1024; static const char * zenoh_shm_message_size_threshold_envar = "ZENOH_SHM_MESSAGE_SIZE_THRESHOLD"; -static const size_t zenoh_shm_message_size_threshold_default = 1; //2 * 1024; +static const size_t zenoh_shm_message_size_threshold_default = 2 * 1024; #endif std::optional _get_z_config( From 65918855dd40b39e0d34b1052cdd03f33357c480 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:27:44 +0300 Subject: [PATCH 09/27] Change 2023 -> 2024 in copyright --- rmw_zenoh_cpp/src/detail/shm_context.cpp | 2 +- rmw_zenoh_cpp/src/detail/shm_context.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/shm_context.cpp b/rmw_zenoh_cpp/src/detail/shm_context.cpp index 5078d4a5..e750fa81 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.cpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Open Source Robotics Foundation, Inc. +// Copyright 2024 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/rmw_zenoh_cpp/src/detail/shm_context.hpp b/rmw_zenoh_cpp/src/detail/shm_context.hpp index 450e2dea..112d7af0 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.hpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Open Source Robotics Foundation, Inc. +// Copyright 2024 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From f12358c7f4f0a9a7a0c233f98822d157a36f205f Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 27 Dec 2024 17:33:58 +0300 Subject: [PATCH 10/27] Remove obsolete comment from default config --- rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 | 2 -- rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 | 2 -- 2 files changed, 4 deletions(-) diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 index b78dc031..8bfb2459 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 @@ -459,8 +459,6 @@ /// A probing procedure for shared memory is performed upon session opening. To enable zenoh to operate /// over shared memory (and to not fallback on network mode), shared memory needs to be enabled also on the /// subscriber side. By doing so, the probing procedure will succeed and shared memory will operate as expected. - /// - /// ROS setting: disabled by default until fully tested enabled: true, }, auth: { diff --git a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 index 42716f24..d04aff2d 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 @@ -462,8 +462,6 @@ /// A probing procedure for shared memory is performed upon session opening. To enable zenoh to operate /// over shared memory (and to not fallback on network mode), shared memory needs to be enabled also on the /// subscriber side. By doing so, the probing procedure will succeed and shared memory will operate as expected. - /// - /// ROS setting: disabled by default until fully tested enabled: true, }, auth: { From 4d9f12240f319bdb9b0981b8a59328c26c916ab3 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 6 Jan 2025 12:24:21 +0300 Subject: [PATCH 11/27] include --- rmw_zenoh_cpp/src/detail/shm_context.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rmw_zenoh_cpp/src/detail/shm_context.hpp b/rmw_zenoh_cpp/src/detail/shm_context.hpp index 112d7af0..8f6a8560 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.hpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.hpp @@ -19,6 +19,8 @@ #include +#include + namespace rmw_zenoh_cpp { ///============================================================================= From bfc298310577d20f7db4575a7ceb38276600b729 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 6 Jan 2025 12:35:28 +0300 Subject: [PATCH 12/27] style fix --- rmw_zenoh_cpp/src/detail/shm_context.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/shm_context.hpp b/rmw_zenoh_cpp/src/detail/shm_context.hpp index 8f6a8560..7443b978 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.hpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.hpp @@ -17,10 +17,10 @@ #ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY -#include - #include +#include + namespace rmw_zenoh_cpp { ///============================================================================= From dfbfec1aa91500e30ecc328d254bd2e57bc1e499 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 6 Jan 2025 15:18:34 +0300 Subject: [PATCH 13/27] bugfixed zenoh --- zenoh_cpp_vendor/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 2649a709..217f6985 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -21,7 +21,7 @@ set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY TRUE CACHE BOOL "Compile Zenoh RMW with S ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git - VCS_VERSION 57d5e4d31d9b38fef34d7bcad3d3e54869c4ce73 + VCS_VERSION 85ca060fa4037239ca4102a3a61f96626cc6b434 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_SHARED_MEMORY:BOOL=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}" @@ -35,7 +35,7 @@ ament_export_dependencies(zenohc) # - https://github.com/eclipse-zenoh/zenoh-cpp/pull/342 (Fix include what you use) ament_vendor(zenoh_cpp_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-cpp - VCS_VERSION 964b64dc8b935a43147287199e7bb12da7b141e6 + VCS_VERSION 8f094255bea28f48d75e2be456e1b06875a8cd04 CMAKE_ARGS -DZENOHCXX_ZENOHC=ON -DZENOHCXX_ZENOHPICO=OFF From 1d8a5e5b6779934f67fafc55a30ddb87978070e1 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Mon, 6 Jan 2025 16:27:15 +0100 Subject: [PATCH 14/27] fix: Update VCS_VERSION for zenoh_cpp_vendor to latest commit (#52) --- zenoh_cpp_vendor/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 217f6985..c113eeef 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -35,7 +35,7 @@ ament_export_dependencies(zenohc) # - https://github.com/eclipse-zenoh/zenoh-cpp/pull/342 (Fix include what you use) ament_vendor(zenoh_cpp_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-cpp - VCS_VERSION 8f094255bea28f48d75e2be456e1b06875a8cd04 + VCS_VERSION 8dce692942246448a9e64f3bae68793634792c33 CMAKE_ARGS -DZENOHCXX_ZENOHC=ON -DZENOHCXX_ZENOHPICO=OFF From fe89b2c5c48cb1d4d39f7c8b07c83c2a135ea925 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 9 Jan 2025 11:46:10 +0300 Subject: [PATCH 15/27] bump zenoh --- zenoh_cpp_vendor/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index c113eeef..24454c8c 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -21,7 +21,7 @@ set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY TRUE CACHE BOOL "Compile Zenoh RMW with S ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git - VCS_VERSION 85ca060fa4037239ca4102a3a61f96626cc6b434 + VCS_VERSION 61d8fcc136ce4ed36d921a32244da4f3d81a6097 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_SHARED_MEMORY:BOOL=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}" @@ -35,7 +35,7 @@ ament_export_dependencies(zenohc) # - https://github.com/eclipse-zenoh/zenoh-cpp/pull/342 (Fix include what you use) ament_vendor(zenoh_cpp_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-cpp - VCS_VERSION 8dce692942246448a9e64f3bae68793634792c33 + VCS_VERSION 05942637c29d3346ad18bab5a178aeebf4be5d62 CMAKE_ARGS -DZENOHCXX_ZENOHC=ON -DZENOHCXX_ZENOHPICO=OFF From 131cac9ec05f793e9bf976108165d3053a42f807 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 9 Jan 2025 13:15:38 +0300 Subject: [PATCH 16/27] - non-blocking allocation for SHM - fallback to non-shm allocation if SHM failed --- rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index 40dc747f..bf690d54 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -252,7 +252,7 @@ rmw_ret_t PublisherData::publish( auto & provider = shm.value().shm_provider; // TODO(yellowhatter): SHM, use alignment based on msgsize_threshold - auto alloc_result = provider.alloc_gc_defrag_blocking( + auto alloc_result = provider.alloc_gc_defrag( max_data_length, zenoh::AllocAlignment({0})); @@ -261,9 +261,15 @@ rmw_ret_t PublisherData::publish( msg_bytes = reinterpret_cast(buf.data()); shmbuf = std::make_optional(std::move(buf)); } else { - // TODO(Yadunund): Should we revert to regular allocation and not return an error? - RMW_SET_ERROR_MSG("Failed to allocate a SHM buffer, even after GCing."); - return RMW_RET_ERROR; + // Print a warning and revert to regular allocation + RMW_ZENOH_LOG_DEBUG_NAMED( + "rmw_zenoh_cpp", "Failed to allocate a SHM buffer, fallback to non-SHM"); + + // TODO(yellowhatter): split the whole publish method onto shm and non-shm versions + // Get memory from the allocator. + msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); } } else { #endif From 89735b129537b1760a828228fe00ebc2e1bb6486 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 10 Jan 2025 11:48:35 +0300 Subject: [PATCH 17/27] change defaulkt shm size to 16mb to fit big topics --- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index dd960529..cd2fcad1 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -47,7 +47,7 @@ static const char * router_check_attempts_envar = "ZENOH_ROUTER_CHECK_ATTEMPTS"; static const char * zenoh_shm_enabled_envar = "ZENOH_SHM_ENABLED"; static const bool zenoh_shm_enabled_default = true; static const char * zenoh_shm_alloc_size_envar = "ZENOH_SHM_ALLOC_SIZE"; -static const size_t zenoh_shm_alloc_size_default = 1 * 1024 * 1024; +static const size_t zenoh_shm_alloc_size_default = 16 * 1024 * 1024; static const char * zenoh_shm_message_size_threshold_envar = "ZENOH_SHM_MESSAGE_SIZE_THRESHOLD"; static const size_t zenoh_shm_message_size_threshold_default = 2 * 1024; #endif From f5520c37dfd55952003f8ba8e55c93d2c9ec75db Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 28 Jan 2025 15:16:19 +0300 Subject: [PATCH 18/27] remove RMW_ZENOH_BUILD_WITH_SHARED_MEMORY --- rmw_zenoh_cpp/CMakeLists.txt | 9 --- .../src/detail/rmw_context_impl_s.cpp | 9 +-- .../src/detail/rmw_context_impl_s.hpp | 2 - .../src/detail/rmw_publisher_data.cpp | 69 ++++++------------- .../src/detail/rmw_publisher_data.hpp | 12 ++-- rmw_zenoh_cpp/src/detail/shm_context.cpp | 4 -- rmw_zenoh_cpp/src/detail/shm_context.hpp | 4 -- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 4 -- rmw_zenoh_cpp/src/detail/zenoh_config.hpp | 2 - rmw_zenoh_cpp/src/rmw_zenoh.cpp | 12 ++-- zenoh_cpp_vendor/CMakeLists.txt | 4 +- 11 files changed, 30 insertions(+), 101 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index c8b36b39..e2b7de21 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -11,8 +11,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY TRUE CACHE BOOL "Compile Zenoh RMW with Shared Memory support") - # find dependencies find_package(ament_cmake REQUIRED) @@ -84,13 +82,6 @@ target_compile_definitions(rmw_zenoh_cpp RMW_VERSION_PATCH=${rmw_VERSION_PATCH} ) -if(${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}) - target_compile_definitions(rmw_zenoh_cpp - PUBLIC - RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - ) -endif() - ament_export_targets(export_rmw_zenoh_cpp) register_rmw_implementation( diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index 797ed049..9a632935 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -165,7 +165,7 @@ class rmw_context_impl_s::Data final : public std::enable_shared_from_this "rmw_zenoh_cpp", "[rmw_context_impl_s] z_call received an invalid reply.\n"); } } -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + // Initialize the shm subsystem if shared_memory is enabled in the config if (rmw_zenoh_cpp::zenoh_shm_enabled()) { RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled"); @@ -178,7 +178,6 @@ class rmw_context_impl_s::Data final : public std::enable_shared_from_this } else { RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is disabled"); } -#endif graph_guard_condition_ = std::make_unique(); graph_guard_condition_->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; @@ -262,13 +261,11 @@ class rmw_context_impl_s::Data final : public std::enable_shared_from_this return session_; } -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY std::optional & shm() { std::lock_guard lock(mutex_); return shm_; } -#endif rmw_guard_condition_t * graph_guard_condition() { @@ -400,11 +397,9 @@ class rmw_context_impl_s::Data final : public std::enable_shared_from_this std::string enclave_; // An owned session. std::shared_ptr session_; -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // An optional SHM context that is initialized of SHM is enabled in the // zenoh session config. std::optional shm_; -#endif // Graph cache. std::shared_ptr graph_cache_; // ROS graph liveliness subscriber. @@ -460,13 +455,11 @@ const std::shared_ptr rmw_context_impl_s::session() const return data_->session(); } -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY ///============================================================================= std::optional & rmw_context_impl_s::shm() { return data_->shm(); } -#endif ///============================================================================= rmw_guard_condition_t * rmw_context_impl_s::graph_guard_condition() diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp index 3c3b9eb7..4a019b7a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp @@ -50,13 +50,11 @@ struct rmw_context_impl_s final // Loan the Zenoh session. const std::shared_ptr session() const; -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // Get a reference to the shm subsystem. // Note: This is not thread-safe. // TODO(Yadunund): Remove this API and instead include a publish() API // that handles the shm_provider once the context manages publishers. std::optional & shm(); -#endif // Get the graph guard condition. rmw_guard_condition_t * graph_guard_condition(); diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index bf690d54..f7f77a7a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -201,10 +201,8 @@ PublisherData::PublisherData( ///============================================================================= rmw_ret_t PublisherData::publish( - const void * ros_message -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - , const std::optional & shm -#endif + const void * ros_message, + const std::optional & shm ) { std::lock_guard lock(mutex_); @@ -221,30 +219,17 @@ rmw_ret_t PublisherData::publish( // To store serialized message byte array. char * msg_bytes = nullptr; -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY std::optional shmbuf = std::nullopt; -#endif rcutils_allocator_t * allocator = &rmw_node_->context->options.allocator; auto always_free_msg_bytes = rcpputils::make_scope_exit( - [&msg_bytes, allocator -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - , &shmbuf -#endif - ]() { - if (msg_bytes -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - && !shmbuf.has_value() -#endif - ) - { + [&msg_bytes, allocator, &shmbuf]() { + if (msg_bytes && !shmbuf.has_value()) { allocator->deallocate(msg_bytes, allocator->state); } }); - -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // Get memory from SHM buffer if available. if (shm.has_value() && max_data_length >= shm.value().msgsize_threshold) { RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled."); @@ -272,14 +257,11 @@ rmw_ret_t PublisherData::publish( msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); } } else { -#endif - // Get memory from the allocator. - msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); - RMW_CHECK_FOR_NULL_WITH_MSG( - msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY -} -#endif + // Get memory from the allocator. + msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); + RMW_CHECK_FOR_NULL_WITH_MSG( + msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); + } // Object that manages the raw buffer eprosima::fastcdr::FastBuffer fastbuffer(msg_bytes, max_data_length); @@ -306,10 +288,7 @@ rmw_ret_t PublisherData::publish( options.attachment = rmw_zenoh_cpp::AttachmentData( sequence_number_++, source_timestamp, entity_->copy_gid()).serialize_to_zbytes(); - auto payload = -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - shmbuf.has_value() ? zenoh::Bytes(std::move(*shmbuf)) : -#endif + auto payload = shmbuf.has_value() ? zenoh::Bytes(std::move(*shmbuf)) : zenoh::Bytes( std::vector( reinterpret_cast(msg_bytes), @@ -334,11 +313,8 @@ rmw_ret_t PublisherData::publish( ///============================================================================= rmw_ret_t PublisherData::publish_serialized_message( - const rmw_serialized_message_t * serialized_message -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - , const std::optional & shm -#endif -) + const rmw_serialized_message_t * serialized_message, + const std::optional & shm) { eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); @@ -360,8 +336,6 @@ rmw_ret_t PublisherData::publish_serialized_message( options.attachment = rmw_zenoh_cpp::AttachmentData( sequence_number_++, source_timestamp, entity_->copy_gid()).serialize_to_zbytes(); - -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY // Get memory from SHM buffer if available. if (shm.has_value() && data_length >= shm.value().msgsize_threshold) { RMW_ZENOH_LOG_DEBUG_NAMED("rmw_zenoh_cpp", "SHM is enabled."); @@ -388,19 +362,16 @@ rmw_ret_t PublisherData::publish_serialized_message( return RMW_RET_ERROR; } } else { -#endif - std::vector raw_image( - serialized_message->buffer, - serialized_message->buffer + data_length); - zenoh::Bytes payload(raw_image); + std::vector raw_image( + serialized_message->buffer, + serialized_message->buffer + data_length); + zenoh::Bytes payload(raw_image); - TRACETOOLS_TRACEPOINT( - rmw_publish, static_cast(rmw_publisher_), serialized_message, source_timestamp); + TRACETOOLS_TRACEPOINT( + rmw_publish, static_cast(rmw_publisher_), serialized_message, source_timestamp); - pub_.put(std::move(payload), std::move(options), &result); -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY -} -#endif + pub_.put(std::move(payload), std::move(options), &result); + } if (result != Z_OK) { if (result == Z_ESESSION_CLOSED) { diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index c27b10e1..33f28d8f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -57,18 +57,14 @@ class PublisherData final // Publish a ROS message. rmw_ret_t publish( - const void * ros_message -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - , const std::optional & shm -#endif + const void * ros_message, + const std::optional & shm ); // Publish a serialized ROS message. rmw_ret_t publish_serialized_message( - const rmw_serialized_message_t * serialized_message -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - , const std::optional & shm -#endif + const rmw_serialized_message_t * serialized_message, + const std::optional & shm ); // Get a copy of the keyexpr_hash of this PublisherData's liveliness::Entity. diff --git a/rmw_zenoh_cpp/src/detail/shm_context.cpp b/rmw_zenoh_cpp/src/detail/shm_context.cpp index e750fa81..2b422d17 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.cpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - #include #include "shm_context.hpp" @@ -29,5 +27,3 @@ ShmContext::ShmContext(size_t alloc_size, size_t msgsize_threshold) msgsize_threshold(msgsize_threshold) {} } // namespace rmw_zenoh_cpp - -#endif diff --git a/rmw_zenoh_cpp/src/detail/shm_context.hpp b/rmw_zenoh_cpp/src/detail/shm_context.hpp index 7443b978..f749c771 100644 --- a/rmw_zenoh_cpp/src/detail/shm_context.hpp +++ b/rmw_zenoh_cpp/src/detail/shm_context.hpp @@ -15,8 +15,6 @@ #ifndef DETAIL__SHM_CONTEXT_HPP_ #define DETAIL__SHM_CONTEXT_HPP_ -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - #include #include @@ -33,6 +31,4 @@ struct ShmContext }; } // namespace rmw_zenoh_cpp -#endif - #endif // DETAIL__SHM_CONTEXT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index cd2fcad1..5c45ca9d 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -43,14 +43,12 @@ static const std::unordered_map _get_z_config( const char * envar_name, @@ -132,7 +130,6 @@ std::optional zenoh_router_check_attempts() return default_value; } -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY ///============================================================================= bool zenoh_shm_enabled() { @@ -220,5 +217,4 @@ size_t zenoh_shm_message_size_threshold() return zenoh_shm_message_size_threshold_default; } -#endif } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp index decab8d7..a44e96cb 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -62,7 +62,6 @@ std::optional get_z_config(const ConfigurableEntity & entity); /// std::nullopt if establishing a connection to a router is not required. std::optional zenoh_router_check_attempts(); -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY ///============================================================================= /// Get the enabled state of shared memory subsystem /// based on the environment variable ZENOH_SHM_ENABLED. @@ -90,7 +89,6 @@ size_t zenoh_shm_alloc_size(); /// - Else value of environemnt variable is returned. /// @return The amount of shared memory to be pre-allocated for Zenoh SHM operation size_t zenoh_shm_message_size_threshold(); -#endif } // namespace rmw_zenoh_cpp #endif // DETAIL__ZENOH_CONFIG_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 9a401d80..3ba6556b 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -618,10 +618,8 @@ rmw_publish( } return pub_data->publish( - ros_message -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - , context_impl->shm() -#endif + ros_message, + context_impl->shm() ); } @@ -728,10 +726,8 @@ rmw_publish_serialized_message( RMW_CHECK_ARGUMENT_FOR_NULL(publisher_data, RMW_RET_INVALID_ARGUMENT); return publisher_data->publish_serialized_message( - serialized_message -#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY - , context_impl->shm() -#endif + serialized_message, + context_impl->shm() ); } diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 88804db0..eb8f28c6 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -17,8 +17,6 @@ find_package(ament_cmake_vendor_package REQUIRED) # when expanded. set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") -set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY TRUE CACHE BOOL "Compile Zenoh RMW with Shared Memory support") - # Set VCS_VERSION to include latest changes from zenoh/zenoh-c to benefit from : # - https://github.com/eclipse-zenoh/zenoh/pull/1685 (Fix deadlock in advanced subscription undeclaration) # - https://github.com/eclipse-zenoh/zenoh/pull/1696 (Fix SHM Garbage Collection (GC) policy) @@ -29,7 +27,7 @@ ament_vendor(zenoh_c_vendor VCS_VERSION 328736fe9bb9b654b1d9f47eecfc6d52f0d7d587 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" - "-DZENOHC_BUILD_WITH_SHARED_MEMORY:BOOL=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY}" + "-DZENOHC_BUILD_WITH_SHARED_MEMORY=TRUE" "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" ) From 681f942404b6e72010c7f11b277c0661311b8b72 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 30 Jan 2025 11:25:15 +0300 Subject: [PATCH 19/27] up zenoh (optimized SHM subsystem) --- zenoh_cpp_vendor/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index eb8f28c6..4b9b08e0 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -24,7 +24,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/transp # - https://github.com/eclipse-zenoh/zenoh/pull/1717 (Improve performance of a large number of peers) ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git - VCS_VERSION 328736fe9bb9b654b1d9f47eecfc6d52f0d7d587 + VCS_VERSION 7a21b9dff6a773d13d8584419506eb42eee2fe4d CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_SHARED_MEMORY=TRUE" @@ -39,7 +39,7 @@ ament_export_dependencies(zenohc) # - https://github.com/eclipse-zenoh/zenoh-cpp/pull/363 (Fix memory leak in string deserialization) ament_vendor(zenoh_cpp_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-cpp - VCS_VERSION bbfef04e843289aae70b5aa060a925e8ee5b1b6f + VCS_VERSION a444049d0dc9443adf610a733aa637f1a330d88e CMAKE_ARGS -DZENOHCXX_ZENOHC=ON -DZENOHCXX_ZENOHPICO=OFF From ccadc0c7fd950612d2520a5ce07518048c4eab5f Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 30 Jan 2025 11:53:58 +0300 Subject: [PATCH 20/27] Update CMakeLists.txt --- zenoh_cpp_vendor/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 4b9b08e0..0be3d2c3 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -43,6 +43,8 @@ ament_vendor(zenoh_cpp_vendor CMAKE_ARGS -DZENOHCXX_ZENOHC=ON -DZENOHCXX_ZENOHPICO=OFF + -DZENOHC_BUILD_WITH_SHARED_MEMORY=TRUE + -DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE ) externalproject_add_stepdependencies(zenoh_cpp_vendor configure zenoh_c_vendor) From 3af6854ca7726780c69eb75bd3249793cc0a63e7 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 30 Jan 2025 19:00:54 +0300 Subject: [PATCH 21/27] Update CMakeLists.txt --- zenoh_cpp_vendor/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 0be3d2c3..6c704f5e 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ament_cmake_vendor_package REQUIRED) # Note: We separate the two args needed for cargo with "$" and not ";" as the # latter is a list separater in cmake and hence the string will be split into two # when expanded. -set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") +set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/unstable zenoh/shared-memory zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") # Set VCS_VERSION to include latest changes from zenoh/zenoh-c to benefit from : # - https://github.com/eclipse-zenoh/zenoh/pull/1685 (Fix deadlock in advanced subscription undeclaration) From 61056c5be2ee1339a14c83f28519b0973a268f3b Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 31 Jan 2025 12:32:47 +0300 Subject: [PATCH 22/27] up zenoh --- zenoh_cpp_vendor/CMakeLists.txt | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 6c704f5e..5d388433 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -24,12 +24,12 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/unstab # - https://github.com/eclipse-zenoh/zenoh/pull/1717 (Improve performance of a large number of peers) ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git - VCS_VERSION 7a21b9dff6a773d13d8584419506eb42eee2fe4d + VCS_VERSION 9844b6988a0c11821c52874d56825058ac732a9f CMAKE_ARGS - "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" - "-DZENOHC_BUILD_WITH_SHARED_MEMORY=TRUE" - "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" - "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" + -DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS} + -DZENOHC_BUILD_WITH_SHARED_MEMORY=TRUE + -DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE + -DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET} ) ament_export_dependencies(zenohc) @@ -39,8 +39,9 @@ ament_export_dependencies(zenohc) # - https://github.com/eclipse-zenoh/zenoh-cpp/pull/363 (Fix memory leak in string deserialization) ament_vendor(zenoh_cpp_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-cpp - VCS_VERSION a444049d0dc9443adf610a733aa637f1a330d88e + VCS_VERSION 86119ade03e46a51a02384471bf44de9daae9793 CMAKE_ARGS + -DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS} -DZENOHCXX_ZENOHC=ON -DZENOHCXX_ZENOHPICO=OFF -DZENOHC_BUILD_WITH_SHARED_MEMORY=TRUE From c6d4a29d872fcdaf6f6484511892b1dbf99b79a1 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 3 Feb 2025 18:35:56 +0300 Subject: [PATCH 23/27] Update CMakeLists.txt --- zenoh_cpp_vendor/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 5d388433..0c757719 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ament_cmake_vendor_package REQUIRED) # Note: We separate the two args needed for cargo with "$" and not ";" as the # latter is a list separater in cmake and hence the string will be split into two # when expanded. -set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/unstable zenoh/shared-memory zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") +set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") # Set VCS_VERSION to include latest changes from zenoh/zenoh-c to benefit from : # - https://github.com/eclipse-zenoh/zenoh/pull/1685 (Fix deadlock in advanced subscription undeclaration) From 42c18e0e7bef0e75c509cf6f526495cd32c16325 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 4 Feb 2025 11:40:42 +0300 Subject: [PATCH 24/27] Update CMakeLists.txt --- zenoh_cpp_vendor/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 0c757719..85cb6ac1 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -49,6 +49,7 @@ ament_vendor(zenoh_cpp_vendor ) externalproject_add_stepdependencies(zenoh_cpp_vendor configure zenoh_c_vendor) +externalproject_add_stepdependencies(zenoh_cpp_vendor build zenoh_c_vendor) ament_export_dependencies(zenohcxx) From 86bcb3aeade691836ee2662fc809551421e82aa9 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 4 Feb 2025 12:16:47 +0300 Subject: [PATCH 25/27] Update CMakeLists.txt --- zenoh_cpp_vendor/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 85cb6ac1..685b9377 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -48,8 +48,8 @@ ament_vendor(zenoh_cpp_vendor -DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE ) -externalproject_add_stepdependencies(zenoh_cpp_vendor configure zenoh_c_vendor) -externalproject_add_stepdependencies(zenoh_cpp_vendor build zenoh_c_vendor) +externalproject_add_stepdependencies(zenoh_cpp_vendor configure zenoh_c) +externalproject_add_stepdependencies(zenoh_cpp_vendor build zenoh_c) ament_export_dependencies(zenohcxx) From 6165770db92307fd5b0c554067829bdb403b0b57 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 4 Feb 2025 13:35:01 +0300 Subject: [PATCH 26/27] Update CMakeLists.txt --- zenoh_cpp_vendor/CMakeLists.txt | 26 ++++++++++---------------- 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/zenoh_cpp_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt index 685b9377..fb771730 100644 --- a/zenoh_cpp_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ament_cmake_vendor_package REQUIRED) # Note: We separate the two args needed for cargo with "$" and not ";" as the # latter is a list separater in cmake and hence the string will be split into two # when expanded. -set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") +set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memory zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") # Set VCS_VERSION to include latest changes from zenoh/zenoh-c to benefit from : # - https://github.com/eclipse-zenoh/zenoh/pull/1685 (Fix deadlock in advanced subscription undeclaration) @@ -24,12 +24,11 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/transp # - https://github.com/eclipse-zenoh/zenoh/pull/1717 (Improve performance of a large number of peers) ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git - VCS_VERSION 9844b6988a0c11821c52874d56825058ac732a9f + VCS_VERSION 15d56e1fb44eead6611cf847f503ef030aee5287 CMAKE_ARGS - -DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS} - -DZENOHC_BUILD_WITH_SHARED_MEMORY=TRUE - -DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE - -DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET} + "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" + "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" + "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" ) ament_export_dependencies(zenohc) @@ -39,18 +38,13 @@ ament_export_dependencies(zenohc) # - https://github.com/eclipse-zenoh/zenoh-cpp/pull/363 (Fix memory leak in string deserialization) ament_vendor(zenoh_cpp_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-cpp - VCS_VERSION 86119ade03e46a51a02384471bf44de9daae9793 + VCS_VERSION bd4d741c6c4fa6509d8d745e22c3c50b4306bd65 CMAKE_ARGS - -DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS} - -DZENOHCXX_ZENOHC=ON - -DZENOHCXX_ZENOHPICO=OFF - -DZENOHC_BUILD_WITH_SHARED_MEMORY=TRUE - -DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE - ) + -DZENOHCXX_ZENOHC=OFF +) -externalproject_add_stepdependencies(zenoh_cpp_vendor configure zenoh_c) -externalproject_add_stepdependencies(zenoh_cpp_vendor build zenoh_c) +externalproject_add_stepdependencies(zenoh_cpp_vendor configure zenoh_c_vendor) ament_export_dependencies(zenohcxx) -ament_package() +ament_package() \ No newline at end of file From 30a00fa0440a44b7ccbe2914888cf7e3acefadb6 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 4 Feb 2025 18:11:42 +0300 Subject: [PATCH 27/27] change default message size threshold fro SHM --- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 2 +- rmw_zenoh_cpp/src/detail/zenoh_config.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 5c45ca9d..14e6b3e3 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -48,7 +48,7 @@ static const bool zenoh_shm_enabled_default = true; static const char * zenoh_shm_alloc_size_envar = "ZENOH_SHM_ALLOC_SIZE"; static const size_t zenoh_shm_alloc_size_default = 16 * 1024 * 1024; static const char * zenoh_shm_message_size_threshold_envar = "ZENOH_SHM_MESSAGE_SIZE_THRESHOLD"; -static const size_t zenoh_shm_message_size_threshold_default = 2 * 1024; +static const size_t zenoh_shm_message_size_threshold_default = 512; std::optional _get_z_config( const char * envar_name, diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp index a44e96cb..b8d1c9ee 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -85,7 +85,7 @@ size_t zenoh_shm_alloc_size(); /// variable ZENOH_SHM_MESSAGE_SIZE_THRESHOLD. /// Messages smaller than this threshold will not be forwarded through Zenoh SHM /// @details The behavior is as follows: -/// - If not set or <= 0, the default value of 2KB is returned. +/// - If not set or <= 0, the default value of 512B is returned. /// - Else value of environemnt variable is returned. /// @return The amount of shared memory to be pre-allocated for Zenoh SHM operation size_t zenoh_shm_message_size_threshold();