diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..722d5e71 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 89b5a598..2e49c698 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) @@ -47,6 +49,7 @@ add_library(rmw_zenoh_cpp SHARED src/detail/zenoh_config.cpp src/detail/zenoh_router_check.cpp src/detail/zenoh_utils.cpp + src/detail/shm_context.cpp src/rmw_event.cpp src/rmw_get_network_flow_endpoints.cpp src/rmw_get_node_info_and_types.cpp @@ -80,6 +83,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 f55c8434..cfec676f 100644 --- a/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 +++ b/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 @@ -457,9 +457,7 @@ /// 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: 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..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,9 +462,7 @@ /// 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: 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 7915cd9e..973862f8 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -164,31 +164,20 @@ class rmw_context_impl_s::Data final } z_drop(z_move(handler)); - // Initialize the shm manager if shared_memory is enabled in the config. - shm_provider_ = std::nullopt; - if (strncmp( - z_string_data(z_loan(shm_enabled)), - "true", - z_string_len(z_loan(shm_enabled))) == 0) - { - // TODO(yuyuan): determine the default alignment of SHM - z_alloc_alignment_t alignment = {5}; - z_owned_memory_layout_t layout; - z_memory_layout_new(&layout, SHM_BUFFER_SIZE_MB * 1024 * 1024, alignment); - - z_owned_shm_provider_t provider; - if (z_posix_shm_provider_new(&provider, z_loan(layout)) != Z_OK) { - RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create an SHM provider."); - throw std::runtime_error("Unable to create an SHM provider."); - } - shm_provider_ = 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"); } - auto free_shm_provider = rcpputils::make_scope_exit( - [this]() { - if (shm_provider_.has_value()) { - z_drop(z_move(shm_provider_.value())); - } - }); +#endif graph_guard_condition_ = std::make_unique(); graph_guard_condition_->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; @@ -216,7 +205,6 @@ class rmw_context_impl_s::Data final } close_session.cancel(); - free_shm_provider.cancel(); undeclare_z_sub.cancel(); } @@ -231,9 +219,10 @@ class rmw_context_impl_s::Data final } z_undeclare_subscriber(z_move(graph_subscriber_)); - if (shm_provider_.has_value()) { - z_drop(z_move(shm_provider_.value())); - } +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + // drop SHM subsystem if used + shm_ = std::nullopt; +#endif is_shutdown_ = true; // We specifically do *not* hold the mutex_ while tearing down the session; this allows us @@ -260,11 +249,13 @@ class rmw_context_impl_s::Data final return z_loan(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() { @@ -396,9 +387,6 @@ class rmw_context_impl_s::Data final std::string enclave_; // An owned session. z_owned_session_t session_; - // An optional SHM manager that is initialized of SHM is enabled in the - // zenoh session config. - std::optional shm_provider_; // Graph cache. std::shared_ptr graph_cache_; // ROS graph liveliness subscriber. @@ -414,6 +402,11 @@ class rmw_context_impl_s::Data final std::size_t next_entity_id_; // Nodes created from this context. std::unordered_map> nodes_; +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + // An optional SHM context that is initialized if SHM is enabled in the + // zenoh session config. + std::optional shm_; +#endif }; ///============================================================================= @@ -477,10 +470,12 @@ const z_loaned_session_t * rmw_context_impl_s::session() const } ///============================================================================= -std::optional & rmw_context_impl_s::shm_provider() +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +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 1e46d6af..2e6bea64 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 @@ class rmw_context_impl_s final // Loan the Zenoh session. const z_loaned_session_t * 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 db096144..9b572e73 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -226,8 +226,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 + , std::optional & shm +#endif +) { std::lock_guard lock(mutex_); if (is_shutdown_) { @@ -236,12 +239,14 @@ rmw_ret_t PublisherData::publish( } // Serialize data. - size_t max_data_length = type_support_->get_estimated_serialized_size( + const size_t max_data_length = type_support_->get_estimated_serialized_size( ros_message, type_support_impl_); // To store serialized message byte array. char * msg_bytes = nullptr; + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY std::optional shmbuf = std::nullopt; auto always_free_shmbuf = rcpputils::make_scope_exit( [&shmbuf]() { @@ -249,25 +254,37 @@ rmw_ret_t PublisherData::publish( z_drop(z_move(shmbuf.value())); } }); +#endif rcutils_allocator_t * allocator = &rmw_node_->context->options.allocator; auto always_free_msg_bytes = rcpputils::make_scope_exit( - [&msg_bytes, allocator, &shmbuf]() { - if (msg_bytes && !shmbuf.has_value()) { + [&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_provider.has_value()) { + 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_provider.value(); + auto & provider = shm.value().shm_provider; + + // TODO(yellowhatter): SHM, use alignment based on msgsize_threshold + z_alloc_alignment_t alignment = {0}; 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), max_data_length, alignment); if (alloc.status == ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { shmbuf = std::make_optional(alloc.buf); @@ -278,11 +295,14 @@ rmw_ret_t PublisherData::publish( return RMW_RET_ERROR; } } else { - // Get memory from the allocator. - msg_bytes = static_cast(allocator->allocate(max_data_length, allocator->state)); - RMW_CHECK_FOR_NULL_WITH_MSG( - msg_bytes, "bytes for message is null", return RMW_RET_BAD_ALLOC); - } +#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); @@ -312,11 +332,16 @@ rmw_ret_t PublisherData::publish( options.attachment = z_move(attachment); z_owned_bytes_t payload; +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + always_free_shmbuf.cancel(); if (shmbuf.has_value()) { z_bytes_from_shm_mut(&payload, z_move(shmbuf.value())); } else { - z_bytes_copy_from_buf(&payload, reinterpret_cast(msg_bytes), data_length); - } +#endif + z_bytes_copy_from_buf(&payload, reinterpret_cast(msg_bytes), data_length); +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +} +#endif z_result_t res = z_publisher_put(z_loan(pub_), z_move(payload), &options); if (res != Z_OK) { @@ -335,8 +360,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 + , std::optional & shm +#endif +) { eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); @@ -363,8 +391,35 @@ rmw_ret_t PublisherData::publish_serialized_message( options.attachment = z_move(attachment); + z_owned_bytes_t payload; +#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 + z_alloc_alignment_t alignment = {0}; + z_buf_layout_alloc_result_t alloc; + z_shm_provider_alloc_gc_defrag_blocking(&alloc, z_loan(provider), data_length, alignment); + + if (alloc.status == ZC_BUF_LAYOUT_ALLOC_STATUS_OK) { + auto msg_bytes = reinterpret_cast(z_shm_mut_data_mut(z_loan_mut(alloc.buf))); + memcpy(msg_bytes, serialized_message->buffer, data_length); + z_bytes_from_shm_mut(&payload, z_move(alloc.buf)); + } else { + // TODO(Yadunund): Should we revert to regular allocation and not return an error? + RMW_SET_ERROR_MSG("Failed to allocate a SHM buffer, even after GCing."); + return RMW_RET_ERROR; + } + } else { +#endif z_bytes_copy_from_buf(&payload, serialized_message->buffer, data_length); +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +} +#endif z_result_t res = z_publisher_put(z_loan(pub_), z_move(payload), &options); if (res != Z_OK) { diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index 4186f434..e475aa3f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -27,6 +27,7 @@ #include "event.hpp" #include "liveliness_utils.hpp" #include "message_type_support.hpp" +#include "shm_context.hpp" #include "type_support_common.hpp" #include "rcutils/allocator.h" @@ -53,13 +54,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 + , 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 + , 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 6d17fbf4..d515ee1d 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp @@ -34,6 +34,7 @@ #include "message_type_support.hpp" #include "attachment_helpers.hpp" #include "type_support_common.hpp" +#include "shm_context.hpp" #include "rcutils/allocator.h" @@ -74,8 +75,11 @@ class SubscriptionData final : public std::enable_shared_from_this & shm_provider); + const void * ros_message +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY + , 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..ccdfe896 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/shm_context.cpp @@ -0,0 +1,67 @@ +// 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) +: msgsize_threshold(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 + z_alloc_alignment_t alignment = {0}; + z_owned_memory_layout_t layout; + if (z_memory_layout_new(&layout, alloc_size, alignment) != Z_OK) { + throw std::runtime_error("Unable to create a Layout for SHM provider."); + } + // Create SHM provider + const auto provider_creation_result = + z_posix_shm_provider_new(&shm_provider, z_loan(layout)); + z_drop(z_move(layout)); + if (provider_creation_result != Z_OK) { + throw std::runtime_error("Unable to create an SHM provider."); + } +} + + +ShmContext::ShmContext(ShmContext && other) +: shm_provider(other.shm_provider), + msgsize_threshold(other.msgsize_threshold) +{ + ::z_internal_null(&other.shm_provider); +} + +ShmContext & ShmContext::operator=(ShmContext && other) +{ + if (this != &other) { + ::z_drop(::z_move(shm_provider)); + shm_provider = other.shm_provider; + ::z_internal_null(&other.shm_provider); + msgsize_threshold = other.msgsize_threshold; + } + return *this; +} + +ShmContext::~ShmContext() +{ + z_drop(z_move(shm_provider)); +} +#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..82eb7234 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/shm_context.hpp @@ -0,0 +1,39 @@ +// 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 +{ + z_owned_shm_provider_t shm_provider; + size_t msgsize_threshold; + + ShmContext(size_t alloc_size, size_t msgsize_threshold); + ShmContext(ShmContext && other); + + ShmContext & operator=(ShmContext && other); + + ~ShmContext(); +}; +#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 2537466b..b653010b 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -40,6 +40,14 @@ static const std::unordered_map 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 dceafe5d..8ca25aed 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -60,6 +60,36 @@ rmw_ret_t get_z_config(const ConfigurableEntity & entity, z_owned_config_t * con /// @return The number of times to try connecting to a zenoh router and /// std::nullopt if establishing a connection to a router is not required. std::optional zenoh_router_check_attempts(); + +#ifdef RMW_ZENOH_BUILD_WITH_SHARED_MEMORY +///============================================================================= +/// Get the 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 885baaf6..ffab4a3c 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -599,8 +599,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 + ); } //============================================================================== @@ -706,8 +709,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_c_vendor/CMakeLists.txt b/zenoh_c_vendor/CMakeLists.txt index 086ea5db..736017c3 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_c_vendor/CMakeLists.txt @@ -15,7 +15,9 @@ find_package(ament_cmake_vendor_package REQUIRED) # Note: We separate the two args needed for cargo with "$" and not ";" as the # latter is a list separater in cmake and hence the string will be split into two # when expanded. -set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memory zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") +set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=zenoh/transport_compression zenoh/transport_tcp zenoh/transport_tls") + +set(RMW_ZENOH_BUILD_WITH_SHARED_MEMORY TRUE CACHE BOOL "Compile Zenoh RMW with Shared Memory support") # Set VCS_VERSION to include latest changes from zenoh-c to benefit from : # - https://github.com/eclipse-zenoh/zenoh-c/pull/340 (fix build issue) @@ -28,9 +30,10 @@ ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git VCS_VERSION 4b759d4e4d35a97d7b20b5c4003b8b764a10f679 CMAKE_ARGS - "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" - "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" - "-DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET}" + -DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS} + -DZENOHC_BUILD_WITH_SHARED_MEMORY:BOOL=${RMW_ZENOH_BUILD_WITH_SHARED_MEMORY} + -DZENOHC_BUILD_WITH_UNSTABLE_API:BOOL=TRUE + -DZENOHC_CUSTOM_TARGET=${ZENOHC_CUSTOM_TARGET} ) ament_export_dependencies(zenohc)