From f7c8938db74c0e038aa92ebddea54430cce00ddf Mon Sep 17 00:00:00 2001 From: CY Chen Date: Thu, 4 Jun 2026 22:15:40 +0000 Subject: [PATCH] Store rosidl buffer backend metadata in graph cache Signed-off-by: CY Chen --- .../include/rmw_dds_common/graph_cache.hpp | 17 ++++-- rmw_dds_common/src/graph_cache.cpp | 60 ++++++++++++++++--- 2 files changed, 65 insertions(+), 12 deletions(-) diff --git a/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp b/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp index 9eb7ed4..f7b18df 100644 --- a/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp +++ b/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp @@ -103,7 +103,8 @@ class GraphCache const rosidl_type_hash_t & type_hash, const rmw_gid_t & participant_gid, const rmw_qos_profile_t & qos, - const rosidl_type_hash_t * service_type_hash = nullptr); + const rosidl_type_hash_t * service_type_hash = nullptr, + const std::unordered_map & buffer_backend_metadata = {}); /// Add a data reader based on discovery. /** @@ -127,7 +128,8 @@ class GraphCache const rosidl_type_hash_t & type_hash, const rmw_gid_t & participant_gid, const rmw_qos_profile_t & qos, - const rosidl_type_hash_t * service_type_hash = nullptr); + const rosidl_type_hash_t * service_type_hash = nullptr, + const std::unordered_map & buffer_backend_metadata = {}); /// Add a data reader or writer. /** @@ -153,7 +155,8 @@ class GraphCache const rmw_gid_t & participant_gid, const rmw_qos_profile_t & qos, bool is_reader, - const rosidl_type_hash_t * service_type_hash = nullptr); + const rosidl_type_hash_t * service_type_hash = nullptr, + const std::unordered_map & buffer_backend_metadata = {}); /// Remove a data writer. /** @@ -611,6 +614,8 @@ struct EntityInfo rmw_gid_t participant_gid; /// Quality of service of the topic. rmw_qos_profile_t qos; + /// Buffer backend metadata advertised for this endpoint. + std::unordered_map buffer_backend_metadata; /// Simple constructor. EntityInfo( @@ -618,12 +623,14 @@ struct EntityInfo const std::string & topic_type, const rosidl_type_hash_t & topic_type_hash, const rmw_gid_t & participant_gid, - const rmw_qos_profile_t & qos) + const rmw_qos_profile_t & qos, + const std::unordered_map & buffer_backend_metadata = {}) : topic_name(topic_name), topic_type(topic_type), topic_type_hash(topic_type_hash), participant_gid(participant_gid), - qos(qos) + qos(qos), + buffer_backend_metadata(buffer_backend_metadata) {} }; diff --git a/rmw_dds_common/src/graph_cache.cpp b/rmw_dds_common/src/graph_cache.cpp index 3fe5255..0acd582 100644 --- a/rmw_dds_common/src/graph_cache.cpp +++ b/rmw_dds_common/src/graph_cache.cpp @@ -31,6 +31,8 @@ #include #include "rcutils/strdup.h" +#include "rcutils/error_handling.h" +#include "rcutils/types/string_map.h" #include "rmw/convert_rcutils_ret_to_rmw_ret.h" #include "rmw/error_handling.h" @@ -70,13 +72,15 @@ GraphCache::add_writer( const rosidl_type_hash_t & type_hash, const rmw_gid_t & participant_gid, const rmw_qos_profile_t & qos, - const rosidl_type_hash_t * service_type_hash) + const rosidl_type_hash_t * service_type_hash, + const std::unordered_map & buffer_backend_metadata) { std::lock_guard guard(mutex_); auto pair = data_writers_.emplace( std::piecewise_construct, std::forward_as_tuple(gid), - std::forward_as_tuple(topic_name, type_name, type_hash, participant_gid, qos)); + std::forward_as_tuple( + topic_name, type_name, type_hash, participant_gid, qos, buffer_backend_metadata)); if (service_type_hash) { data_services_.emplace( std::piecewise_construct, @@ -95,13 +99,15 @@ GraphCache::add_reader( const rosidl_type_hash_t & type_hash, const rmw_gid_t & participant_gid, const rmw_qos_profile_t & qos, - const rosidl_type_hash_t * service_type_hash) + const rosidl_type_hash_t * service_type_hash, + const std::unordered_map & buffer_backend_metadata) { std::lock_guard guard(mutex_); auto pair = data_readers_.emplace( std::piecewise_construct, std::forward_as_tuple(gid), - std::forward_as_tuple(topic_name, type_name, type_hash, participant_gid, qos)); + std::forward_as_tuple( + topic_name, type_name, type_hash, participant_gid, qos, buffer_backend_metadata)); if (service_type_hash) { data_services_.emplace( std::piecewise_construct, @@ -121,7 +127,8 @@ GraphCache::add_entity( const rmw_gid_t & participant_gid, const rmw_qos_profile_t & qos, bool is_reader, - const rosidl_type_hash_t * service_type_hash) + const rosidl_type_hash_t * service_type_hash, + const std::unordered_map & buffer_backend_metadata) { if (is_reader) { return this->add_reader( @@ -131,7 +138,8 @@ GraphCache::add_entity( type_hash, participant_gid, qos, - service_type_hash); + service_type_hash, + buffer_backend_metadata); } return this->add_writer( gid, @@ -140,7 +148,8 @@ GraphCache::add_entity( type_hash, participant_gid, qos, - service_type_hash); + service_type_hash, + buffer_backend_metadata); } bool @@ -622,6 +631,43 @@ __get_entities_info_by_topic( if (RMW_RET_OK != ret) { return ret; } + + if (!entity_pair.second.buffer_backend_metadata.empty()) { + rcutils_string_map_t buffer_backend_metadata = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcutils_ret = rcutils_string_map_init( + &buffer_backend_metadata, + entity_pair.second.buffer_backend_metadata.size(), + *allocator); + if (RCUTILS_RET_OK != rcutils_ret) { + RMW_SET_ERROR_MSG(rcutils_get_error_string().str); + rcutils_reset_error(); + return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + } + + for (const auto & backend_pair : entity_pair.second.buffer_backend_metadata) { + rcutils_ret = rcutils_string_map_set( + &buffer_backend_metadata, + backend_pair.first.c_str(), + backend_pair.second.c_str()); + if (RCUTILS_RET_OK != rcutils_ret) { + rcutils_ret_t fini_ret = rcutils_string_map_fini(&buffer_backend_metadata); + (void)fini_ret; + RMW_SET_ERROR_MSG(rcutils_get_error_string().str); + rcutils_reset_error(); + return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); + } + } + + ret = rmw_topic_endpoint_info_set_buffer_backend_metadata( + &endpoint_info, + &buffer_backend_metadata, + allocator); + rcutils_ret_t fini_ret = rcutils_string_map_fini(&buffer_backend_metadata); + (void)fini_ret; + if (RMW_RET_OK != ret) { + return ret; + } + } i++; }