diff --git a/rmw_dds_common/CMakeLists.txt b/rmw_dds_common/CMakeLists.txt index b930cc3..a85bbeb 100644 --- a/rmw_dds_common/CMakeLists.txt +++ b/rmw_dds_common/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(rosidl_runtime_c REQUIRED) ament_add_default_options() ament_export_dependencies(ament_cmake_core) @@ -44,7 +45,8 @@ target_link_libraries(${PROJECT_NAME}_library PUBLIC rcutils::rcutils rmw::rmw) target_link_libraries(${PROJECT_NAME}_library PRIVATE - rcpputils::rcpputils) + rcpputils::rcpputils + rosidl_runtime_c::rosidl_runtime_c) target_include_directories(${PROJECT_NAME}_library PUBLIC "$" @@ -95,7 +97,8 @@ if(BUILD_TESTING) ament_add_gmock(test_graph_cache test/test_graph_cache.cpp) if(TARGET test_graph_cache) - target_link_libraries(test_graph_cache ${PROJECT_NAME}_library osrf_testing_tools_cpp::memory_tools) + target_link_libraries(test_graph_cache + ${PROJECT_NAME}_library osrf_testing_tools_cpp::memory_tools rosidl_runtime_c::rosidl_runtime_c) target_compile_definitions(test_graph_cache PUBLIC RCUTILS_ENABLE_FAULT_INJECTION) endif() @@ -106,7 +109,7 @@ if(BUILD_TESTING) ament_add_gmock(test_qos test/test_qos.cpp) if(TARGET test_qos) - target_link_libraries(test_qos ${PROJECT_NAME}_library osrf_testing_tools_cpp::memory_tools) + target_link_libraries(test_qos ${PROJECT_NAME}_library osrf_testing_tools_cpp::memory_tools rcpputils::rcpputils) endif() ament_add_gmock(test_time_utils test/test_time_utils.cpp) @@ -123,7 +126,7 @@ if(BUILD_TESTING) add_performance_test(benchmark_graph_cache test/benchmark/benchmark_graph_cache.cpp) if(TARGET benchmark_graph_cache) - target_link_libraries(benchmark_graph_cache ${PROJECT_NAME}_library) + target_link_libraries(benchmark_graph_cache ${PROJECT_NAME}_library rosidl_runtime_c::rosidl_runtime_c) endif() endif() 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 30802e6..bc08ecf 100644 --- a/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp +++ b/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp @@ -78,11 +78,12 @@ class GraphCache * @{ */ - /// Add a data writer. + /// Add a data writer based on discovery. /** * \param writer_gid GUID of the data writer. * \param topic_name Name of the DDS topic for this data writer. * \param type_name Type name of the DDS topic for this data writer. + * \param type_hash Hash of the description of the topic type. * \param participant_gid GUID of the participant. * \param qos QoS profile of the data writer. * \return `true` if the cache was updated, `false` if the data writer @@ -90,6 +91,22 @@ class GraphCache */ RMW_DDS_COMMON_PUBLIC bool + add_writer( + const rmw_gid_t & writer_gid, + const std::string & topic_name, + const std::string & type_name, + const rosidl_type_hash_t & type_hash, + const rmw_gid_t & participant_gid, + const rmw_qos_profile_t & qos); + + /// Add a data writer based on discovery. + /** + * See add_reader with rosidl_type_hash_t, whose other parameters match these. + */ + RMW_DDS_COMMON_PUBLIC + RCUTILS_DEPRECATED_WITH_MSG( + "Migrate to using the version of this function taking a type hash.") + bool add_writer( const rmw_gid_t & writer_gid, const std::string & topic_name, @@ -102,6 +119,7 @@ class GraphCache * \param reader_gid GUID of the The data reader. * \param topic_name Name of the DDS topic for this data reader. * \param type_name Type name of the DDS topic for this data reader. + * \param type_hash Hash of the description of the topic type. * \param participant_gid GUID of the participant. * \param qos QoS profile of the data reader. * \return `true` if the cache was updated, `false` if the data reader @@ -109,6 +127,22 @@ class GraphCache */ RMW_DDS_COMMON_PUBLIC bool + add_reader( + const rmw_gid_t & reader_gid, + const std::string & topic_name, + const std::string & type_name, + const rosidl_type_hash_t & type_hash, + const rmw_gid_t & participant_gid, + const rmw_qos_profile_t & qos); + + /// Add a data reader based on discovery. + /** + * See add_reader with rosidl_type_hash_t, whose other parameters match these. + */ + RMW_DDS_COMMON_PUBLIC + RCUTILS_DEPRECATED_WITH_MSG( + "Migrate to using the version of this function taking a type hash.") + bool add_reader( const rmw_gid_t & reader_gid, const std::string & topic_name, @@ -121,6 +155,7 @@ class GraphCache * \param gid GUID of the entity. * \param topic_name Name of the DDS topic for this data reader. * \param type_name Type name of the DDS topic for this entity + * \param type_hash Hash of the description of the topic type. * \param participant_gid GUID of the participant. * \param qos QoS profile of the entity. * \param is_reader Whether the entity is a data reader or a writer. @@ -129,6 +164,23 @@ class GraphCache */ RMW_DDS_COMMON_PUBLIC bool + add_entity( + const rmw_gid_t & gid, + const std::string & topic_name, + const std::string & type_name, + const rosidl_type_hash_t & type_hash, + const rmw_gid_t & participant_gid, + const rmw_qos_profile_t & qos, + bool is_reader); + + /// Add a data reader or writer. + /** + * See add_entity with rosidl_type_hash_t, whose other parameters match these. + */ + RMW_DDS_COMMON_PUBLIC + RCUTILS_DEPRECATED_WITH_MSG( + "Migrate to using the version of this function taking a type hash.") + bool add_entity( const rmw_gid_t & gid, const std::string & topic_name, @@ -541,8 +593,10 @@ struct EntityInfo { /// Topic name. std::string topic_name; - /// Topic type. + /// Topic type name. std::string topic_type; + /// Topic type hash. + rosidl_type_hash_t topic_type_hash; /// Participant gid. rmw_gid_t participant_gid; /// Quality of service of the topic. @@ -552,10 +606,12 @@ struct EntityInfo EntityInfo( const std::string & topic_name, 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) : topic_name(topic_name), topic_type(topic_type), + topic_type_hash(topic_type_hash), participant_gid(participant_gid), qos(qos) {} diff --git a/rmw_dds_common/include/rmw_dds_common/qos.hpp b/rmw_dds_common/include/rmw_dds_common/qos.hpp index 3f0b128..63e94f7 100644 --- a/rmw_dds_common/include/rmw_dds_common/qos.hpp +++ b/rmw_dds_common/include/rmw_dds_common/qos.hpp @@ -16,6 +16,7 @@ #define RMW_DDS_COMMON__QOS_HPP_ #include +#include #include "rmw/qos_profiles.h" #include "rmw/topic_endpoint_info_array.h" @@ -224,6 +225,36 @@ RMW_DDS_COMMON_PUBLIC rmw_qos_profile_t qos_profile_update_best_available_for_services(const rmw_qos_profile_t & qos_profile); +/// Parse USER_DATA "key=value;key=value;"" encoding, finding value of key "typehash" +/** + * \param[in] user_data USER_DATA qos raw bytes + * \param[in] user_data_size Length of user_data + * \param[out] type_hash_out Filled with type hash data if found, or to zero value if key not found + * \return RMW_RET_OK if key parsed successfully, or if key not found + * \return RMW_RET_INVALID_ARGUMENT if user_data is null + * \return RMW_RET_ERROR if typehash key found, but value could not be parsed + */ +RMW_DDS_COMMON_PUBLIC +rmw_ret_t +parse_type_hash_from_user_data( + const uint8_t * user_data, + size_t user_data_size, + rosidl_type_hash_t & type_hash_out); + +/// Encode type hash as "typehash=hash_string;" for use in USER_DATA QoS +/** + * \param[in] type_hash Type hash value to encode + * \param[out] string_out On success, will be set to "typehash=stringified_type_hash;" + * If type_hash's version is 0, string_out will be set to empty + * \return RMW_RET_OK on success, including empty string for unset version + * \return RMW_RET_BAD_ALLOC if memory allocation fails + */ +RMW_DDS_COMMON_PUBLIC +rmw_ret_t +encode_type_hash_for_user_data_qos( + const rosidl_type_hash_t & type_hash, + std::string & string_out); + } // namespace rmw_dds_common #endif // RMW_DDS_COMMON__QOS_HPP_ diff --git a/rmw_dds_common/package.xml b/rmw_dds_common/package.xml index b0b57a7..8257e50 100644 --- a/rmw_dds_common/package.xml +++ b/rmw_dds_common/package.xml @@ -20,6 +20,7 @@ rcutils rcpputils rmw + rosidl_runtime_c rosidl_runtime_cpp ament_cmake_gmock diff --git a/rmw_dds_common/src/graph_cache.cpp b/rmw_dds_common/src/graph_cache.cpp index 551bb1b..52f6b20 100644 --- a/rmw_dds_common/src/graph_cache.cpp +++ b/rmw_dds_common/src/graph_cache.cpp @@ -67,6 +67,7 @@ GraphCache::add_writer( const rmw_gid_t & gid, const std::string & topic_name, const std::string & type_name, + const rosidl_type_hash_t & type_hash, const rmw_gid_t & participant_gid, const rmw_qos_profile_t & qos) { @@ -74,16 +75,34 @@ GraphCache::add_writer( auto pair = data_writers_.emplace( std::piecewise_construct, std::forward_as_tuple(gid), - std::forward_as_tuple(topic_name, type_name, participant_gid, qos)); + std::forward_as_tuple(topic_name, type_name, type_hash, participant_gid, qos)); GRAPH_CACHE_CALL_ON_CHANGE_CALLBACK_IF(this, pair.second); return pair.second; } +bool +GraphCache::add_writer( + const rmw_gid_t & gid, + const std::string & topic_name, + const std::string & type_name, + const rmw_gid_t & participant_gid, + const rmw_qos_profile_t & qos) +{ + return this->add_writer( + gid, + topic_name, + type_name, + rosidl_get_zero_initialized_type_hash(), + participant_gid, + qos); +} + bool GraphCache::add_reader( const rmw_gid_t & gid, const std::string & topic_name, const std::string & type_name, + const rosidl_type_hash_t & type_hash, const rmw_gid_t & participant_gid, const rmw_qos_profile_t & qos) { @@ -91,16 +110,34 @@ GraphCache::add_reader( auto pair = data_readers_.emplace( std::piecewise_construct, std::forward_as_tuple(gid), - std::forward_as_tuple(topic_name, type_name, participant_gid, qos)); + std::forward_as_tuple(topic_name, type_name, type_hash, participant_gid, qos)); GRAPH_CACHE_CALL_ON_CHANGE_CALLBACK_IF(this, pair.second); return pair.second; } +bool +GraphCache::add_reader( + const rmw_gid_t & gid, + const std::string & topic_name, + const std::string & type_name, + const rmw_gid_t & participant_gid, + const rmw_qos_profile_t & qos) +{ + return this->add_reader( + gid, + topic_name, + type_name, + rosidl_get_zero_initialized_type_hash(), + participant_gid, + qos); +} + bool GraphCache::add_entity( const rmw_gid_t & gid, const std::string & topic_name, const std::string & type_name, + const rosidl_type_hash_t & type_hash, const rmw_gid_t & participant_gid, const rmw_qos_profile_t & qos, bool is_reader) @@ -110,6 +147,7 @@ GraphCache::add_entity( gid, topic_name, type_name, + type_hash, participant_gid, qos); } @@ -117,10 +155,30 @@ GraphCache::add_entity( gid, topic_name, type_name, + type_hash, participant_gid, qos); } +bool +GraphCache::add_entity( + const rmw_gid_t & gid, + const std::string & topic_name, + const std::string & type_name, + const rmw_gid_t & participant_gid, + const rmw_qos_profile_t & qos, + bool is_reader) +{ + return this->add_entity( + gid, + topic_name, + type_name, + rosidl_get_zero_initialized_type_hash(), + participant_gid, + qos, + is_reader); +} + bool GraphCache::remove_writer(const rmw_gid_t & gid) { @@ -570,6 +628,13 @@ __get_entities_info_by_topic( return ret; } + ret = rmw_topic_endpoint_info_set_topic_type_hash( + &endpoint_info, + &entity_pair.second.topic_type_hash); + if (RMW_RET_OK != ret) { + return ret; + } + ret = rmw_topic_endpoint_info_set_endpoint_type( &endpoint_info, is_reader ? RMW_ENDPOINT_SUBSCRIPTION : RMW_ENDPOINT_PUBLISHER); diff --git a/rmw_dds_common/src/qos.cpp b/rmw_dds_common/src/qos.cpp index da1a8da..7604cb1 100644 --- a/rmw_dds_common/src/qos.cpp +++ b/rmw_dds_common/src/qos.cpp @@ -16,9 +16,14 @@ #include #include +#include +#include +#include "rcpputils/scope_exit.hpp" +#include "rcutils/error_handling.h" #include "rcutils/snprintf.h" #include "rmw/error_handling.h" +#include "rmw/impl/cpp/key_value.hpp" #include "rmw/get_topic_endpoint_info.h" #include "rmw/qos_profiles.h" #include "rmw/qos_string_conversions.h" @@ -664,4 +669,48 @@ qos_profile_update_best_available_for_services(const rmw_qos_profile_t & qos_pro return result; } +rmw_ret_t +parse_type_hash_from_user_data( + const uint8_t * user_data, + size_t user_data_size, + rosidl_type_hash_t & type_hash_out) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(user_data, RMW_RET_INVALID_ARGUMENT); + std::vector udvec(user_data, user_data + user_data_size); + auto key_value = rmw::impl::cpp::parse_key_value(udvec); + auto typehash_it = key_value.find("typehash"); + if (typehash_it == key_value.end()) { + type_hash_out = rosidl_get_zero_initialized_type_hash(); + return RMW_RET_OK; + } + std::string type_hash_str(typehash_it->second.begin(), typehash_it->second.end()); + if (RCUTILS_RET_OK != rosidl_parse_type_hash_string(type_hash_str.c_str(), &type_hash_out)) { + return RMW_RET_ERROR; + } + return RMW_RET_OK; +} + +rmw_ret_t +encode_type_hash_for_user_data_qos( + const rosidl_type_hash_t & type_hash, + std::string & string_out) +{ + if (type_hash.version == ROSIDL_TYPE_HASH_VERSION_UNSET) { + string_out.clear(); + return RMW_RET_OK; + } + auto allocator = rcutils_get_default_allocator(); + char * type_hash_c_str = nullptr; + rcutils_ret_t stringify_ret = rosidl_stringify_type_hash(&type_hash, allocator, &type_hash_c_str); + if (RCUTILS_RET_BAD_ALLOC == stringify_ret) { + return RMW_RET_BAD_ALLOC; + } + if (RCUTILS_RET_OK != stringify_ret) { + return RMW_RET_ERROR; + } + RCPPUTILS_SCOPE_EXIT(allocator.deallocate(type_hash_c_str, &allocator.state)); + string_out = "typehash=" + std::string(type_hash_c_str) + ";"; + return RMW_RET_OK; +} + } // namespace rmw_dds_common diff --git a/rmw_dds_common/test/benchmark/benchmark_graph_cache.cpp b/rmw_dds_common/test/benchmark/benchmark_graph_cache.cpp index 349bb7a..1fe8b55 100644 --- a/rmw_dds_common/test/benchmark/benchmark_graph_cache.cpp +++ b/rmw_dds_common/test/benchmark/benchmark_graph_cache.cpp @@ -24,6 +24,8 @@ #include "rmw_dds_common/gid_utils.hpp" #include "rmw_dds_common/graph_cache.hpp" +#include "rosidl_runtime_c/type_hash.h" + using performance_test_fixture::PerformanceTest; using rmw_dds_common::GraphCache; @@ -120,6 +122,7 @@ add_entities( gid_from_string(elem.gid), elem.name, elem.type, + rosidl_get_zero_initialized_type_hash(), gid_from_string(elem.participant_gid), rmw_qos_profile_default, elem.is_reader); diff --git a/rmw_dds_common/test/test_graph_cache.cpp b/rmw_dds_common/test/test_graph_cache.cpp index b4c4206..c9927ad 100644 --- a/rmw_dds_common/test/test_graph_cache.cpp +++ b/rmw_dds_common/test/test_graph_cache.cpp @@ -303,6 +303,7 @@ add_entities( gid_from_string(elem.gid), elem.name, elem.type, + rosidl_get_zero_initialized_type_hash(), gid_from_string(elem.participant_gid), rmw_qos_profile_default, elem.is_reader)); diff --git a/rmw_dds_common/test/test_qos.cpp b/rmw_dds_common/test/test_qos.cpp index c8aedbb..1074ce7 100644 --- a/rmw_dds_common/test/test_qos.cpp +++ b/rmw_dds_common/test/test_qos.cpp @@ -17,6 +17,7 @@ #include #include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcpputils/scope_exit.hpp" #include "rmw/error_handling.h" #include "rmw/qos_profiles.h" #include "rmw/types.h" @@ -1072,3 +1073,60 @@ TEST(test_qos, test_qos_profile_update_best_available_for_services) rmw_qos_profile_services_default.liveliness_lease_duration, output_profile.liveliness_lease_duration)); } + +TEST(test_qos, test_parse_type_hash_from_user_data) +{ + const auto zero_val = rosidl_get_zero_initialized_type_hash(); + + std::string bad_value = "something that isn't key equals value semicolon"; + rosidl_type_hash_t result_type_hash; + rmw_ret_t ret = rmw_dds_common::parse_type_hash_from_user_data( + reinterpret_cast(bad_value.data()), bad_value.size(), result_type_hash); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(0, memcmp(&result_type_hash, &zero_val, sizeof(rosidl_type_hash_t))) + << "Non-ros 2 user_data should result in zero hash struct."; + + std::string no_key = "key1=value1;key2=value2;key3=value3;"; + ret = rmw_dds_common::parse_type_hash_from_user_data( + reinterpret_cast(no_key.data()), no_key.size(), result_type_hash); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(0, memcmp(&result_type_hash, &zero_val, sizeof(rosidl_type_hash_t))) + << "No typehash key should result in zero hash struct."; + + rosidl_type_hash_t input_type_hash; + input_type_hash.version = 1; + for (uint8_t i = 0; i < ROSIDL_TYPE_HASH_SIZE; i++) { + input_type_hash.value[i] = i; + } + char * type_hash_c_str; + auto allocator = rcutils_get_default_allocator(); + ret = rosidl_stringify_type_hash(&input_type_hash, allocator, &type_hash_c_str); + ASSERT_EQ(ret, RCUTILS_RET_OK); + RCPPUTILS_SCOPE_EXIT(allocator.deallocate(type_hash_c_str, &allocator.state)); + std::string type_hash_string(type_hash_c_str); + std::string good_data = "foo=bar;typehash=" + type_hash_string + ";key=value;"; + ret = rmw_dds_common::parse_type_hash_from_user_data( + reinterpret_cast(good_data.data()), good_data.size(), result_type_hash); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(0, memcmp(&result_type_hash, &input_type_hash, sizeof(rosidl_type_hash_t))); +} + +TEST(test_qos, test_encode_type_hash_for_user_data_qos) +{ + rosidl_type_hash_t test_hash = rosidl_get_zero_initialized_type_hash(); + std::string hash_string; + rmw_ret_t ret = rmw_dds_common::encode_type_hash_for_user_data_qos(test_hash, hash_string); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(hash_string, ""); + + test_hash.version = 1; + for (uint8_t i = 0; i < ROSIDL_TYPE_HASH_SIZE; i++) { + test_hash.value[i] = i; + } + hash_string.clear(); + ret = rmw_dds_common::encode_type_hash_for_user_data_qos(test_hash, hash_string); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ( + hash_string, + "typehash=RIHS01_000102030405060708090a0b0c0d0e0f101112131415161718191a1b1c1d1e1f;"); +}