From aefd05d23fabaed154fdf1d00c458f72e32ecf40 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 14 Feb 2023 16:39:22 -0800 Subject: [PATCH 01/17] WIP just making the branch and noting the place where code goes Signed-off-by: Emerson Knapp --- rmw_fastrtps_cpp/src/publisher.cpp | 2 ++ .../include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 1 + 2 files changed, 3 insertions(+) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index da9de0eb36..3e37eb536f 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -261,6 +261,8 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } + // TODO(emersonknapp) before this create_datawriter is where i must use USER_DATA + // Creates DataWriter with a mask enabling publication_matched calls for the listener info->data_writer_ = publisher->create_datawriter( info->topic_, diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 0f3013ce50..bf71daafe3 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -205,6 +205,7 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); + // TODO(emersonknapp) this is the place! context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( identifier_, From dd124b3a1ba913393e41580dcb6c5c845da09e0d Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 15 Feb 2023 00:01:20 -0800 Subject: [PATCH 02/17] Basic test workflow, not getting actual type hash value yet Signed-off-by: Emerson Knapp --- rmw_fastrtps_cpp/src/publisher.cpp | 10 ++++++++-- .../custom_participant_info.hpp | 11 +++++++++++ 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 3e37eb536f..c8dabd2f4c 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -255,14 +255,20 @@ rmw_fastrtps_cpp::create_publisher( writer_qos.data_sharing().off(); } + // Add type hash information to USER_DATA QoS + // TODO(emersonknapp) how to get this value? + std::string type_hash = "1f2f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f"; + std::string user_data_str = "type_hash=" + type_hash + ";"; + std::vector user_data(user_data_str.begin(), user_data_str.end()); + writer_qos.user_data().set_max_size(user_data.size()); + writer_qos.user_data().setValue(user_data); + // Get QoS from RMW if (!get_datawriter_qos(*qos_policies, writer_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } - // TODO(emersonknapp) before this create_datawriter is where i must use USER_DATA - // Creates DataWriter with a mask enabling publication_matched calls for the listener info->data_writer_ = publisher->create_datawriter( info->topic_, diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index bf71daafe3..41a56a0d67 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -206,12 +206,23 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); // TODO(emersonknapp) this is the place! + const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); + std::string user_data(userDataValue.begin(), userDataValue.end()); + RCUTILS_LOG_ERROR("Discovery: %s @ %s --- %s", + proxyData.topicName().to_string().c_str(), + proxyData.typeName().to_string().c_str(), + user_data.c_str()); + + uint8_t type_hash[RCUTILS_SHA256_BLOCK_SIZE]; + memset(type_hash, 0x2c, RCUTILS_SHA256_BLOCK_SIZE); + context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( identifier_, proxyData.guid()), proxyData.topicName().to_string(), proxyData.typeName().to_string(), + type_hash, rmw_fastrtps_shared_cpp::create_rmw_gid( identifier_, iHandle2GUID(proxyData.RTPSParticipantKey())), From c4b18ff8975cf6988aab85cc9e1dd4cd7e97d877 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 16 Feb 2023 15:10:01 -0800 Subject: [PATCH 03/17] Adding type hash to user data Signed-off-by: Emerson Knapp --- rmw_fastrtps_cpp/src/publisher.cpp | 10 +----- rmw_fastrtps_cpp/src/rmw_client.cpp | 4 +-- rmw_fastrtps_cpp/src/rmw_service.cpp | 4 +-- rmw_fastrtps_cpp/src/subscription.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 4 +-- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 4 +-- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 2 +- .../custom_participant_info.hpp | 7 ++-- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 2 ++ rmw_fastrtps_shared_cpp/src/qos.cpp | 26 ++++++++++++-- .../test/test_rmw_qos_to_dds_attributes.cpp | 36 +++++++++---------- 12 files changed, 59 insertions(+), 44 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index c8dabd2f4c..bb86e6ff11 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -255,16 +255,8 @@ rmw_fastrtps_cpp::create_publisher( writer_qos.data_sharing().off(); } - // Add type hash information to USER_DATA QoS - // TODO(emersonknapp) how to get this value? - std::string type_hash = "1f2f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f1f"; - std::string user_data_str = "type_hash=" + type_hash + ";"; - std::vector user_data(user_data_str.begin(), user_data_str.end()); - writer_qos.user_data().set_max_size(user_data.size()); - writer_qos.user_data().setValue(user_data); - // Get QoS from RMW - if (!get_datawriter_qos(*qos_policies, writer_qos)) { + if (!get_datawriter_qos(*qos_policies, callbacks->type_hash_, writer_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index aa8c23b170..985c0d3e40 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -314,7 +314,7 @@ rmw_create_client( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, response_members->type_hash_, reader_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -368,7 +368,7 @@ rmw_create_client( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, request_members->type_hash_, writer_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 36767afdd5..1bc813315c 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -313,7 +313,7 @@ rmw_create_service( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, request_members->type_hash_, reader_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -371,7 +371,7 @@ rmw_create_service( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, response_members->type_hash_, writer_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 9ce7ac111b..36b17905e8 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -280,7 +280,7 @@ create_subscription( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(*qos_policies, reader_qos)) { + if (!get_datareader_qos(*qos_policies, callbacks->type_hash_, reader_qos)) { RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index cae4a7a132..0472fdef31 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -262,7 +262,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } // Get QoS from RMW - if (!get_datawriter_qos(*qos_policies, writer_qos)) { + if (!get_datawriter_qos(*qos_policies, nullptr, writer_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 3731f850fc..6055b21287 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -345,7 +345,7 @@ rmw_create_client( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, nullptr, reader_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -399,7 +399,7 @@ rmw_create_client( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, nullptr, writer_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 6b5486ff98..b70fd8e943 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -344,7 +344,7 @@ rmw_create_service( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, nullptr, reader_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -402,7 +402,7 @@ rmw_create_service( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, nullptr, writer_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index fc147c0fee..391cef53f0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -264,7 +264,7 @@ create_subscription( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(*qos_policies, reader_qos)) { + if (!get_datareader_qos(*qos_policies, nullptr, reader_qos)) { RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; } diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 41a56a0d67..0e695c9de6 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -205,7 +205,9 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); - // TODO(emersonknapp) this is the place! + uint8_t type_hash[RCUTILS_SHA256_BLOCK_SIZE]; + memset(type_hash, 0x2c, RCUTILS_SHA256_BLOCK_SIZE); + const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); std::string user_data(userDataValue.begin(), userDataValue.end()); RCUTILS_LOG_ERROR("Discovery: %s @ %s --- %s", @@ -213,9 +215,6 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList proxyData.typeName().to_string().c_str(), user_data.c_str()); - uint8_t type_hash[RCUTILS_SHA256_BLOCK_SIZE]; - memset(type_hash, 0x2c, RCUTILS_SHA256_BLOCK_SIZE); - context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( identifier_, diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index ad5799452f..791fb251ab 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -35,12 +35,14 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, + const uint8_t * type_hash, eprosima::fastdds::dds::DataReaderQos & reader_qos); RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datawriter_qos( const rmw_qos_profile_t & qos_policies, + const uint8_t * type_hash, eprosima::fastdds::dds::DataWriterQos & writer_qos); RMW_FASTRTPS_SHARED_CPP_PUBLIC diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 74464b778a..47c59ba2a2 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -142,20 +142,42 @@ bool fill_entity_qos_from_profile( return true; } +template +bool fill_data_entity_qos_from_profile( + const rmw_qos_profile_t & qos_policies, + const uint8_t * type_hash, + DDSEntityQos & entity_qos) +{ + if (!fill_entity_qos_from_profile(qos_policies, entity_qos)) { + return false; + } + + std::string prefix = "type_hash="; + std::vector user_data(prefix.begin(), prefix.end()); + user_data.insert(user_data.end(), type_hash, type_hash + 32); + user_data.push_back(';'); + entity_qos.user_data().resize(user_data.size()); + entity_qos.user_data().setValue(user_data); + + return true; +} + bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, + const uint8_t * type_hash, eprosima::fastdds::dds::DataReaderQos & datareader_qos) { - return fill_entity_qos_from_profile(qos_policies, datareader_qos); + return fill_data_entity_qos_from_profile(qos_policies, type_hash, datareader_qos); } bool get_datawriter_qos( const rmw_qos_profile_t & qos_policies, + const uint8_t * type_hash, eprosima::fastdds::dds::DataWriterQos & datawriter_qos) { - return fill_entity_qos_from_profile(qos_policies, datawriter_qos); + return fill_data_entity_qos_from_profile(qos_policies, type_hash, datawriter_qos); } bool diff --git a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp index b080a1393b..825991431b 100644 --- a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp @@ -39,28 +39,28 @@ class GetDataReaderQoSTest : public ::testing::Test TEST_F(GetDataReaderQoSTest, test_unknown_history_policy_conversion_fails) { qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_reliability_policy_conversion_fails) { qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_durability_policy_conversion_fails) { qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_liveliness_policy_conversion_fails) { qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } @@ -78,7 +78,7 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { qos_profile_.liveliness_lease_duration.sec = 10u; qos_profile_.liveliness_lease_duration.nsec = 0u; - EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); EXPECT_EQ( eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, @@ -106,7 +106,7 @@ TEST_F(GetDataReaderQoSTest, large_depth_conversion) { qos_profile_.depth = depth; qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); EXPECT_EQ( eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, @@ -117,12 +117,12 @@ TEST_F(GetDataReaderQoSTest, large_depth_conversion) { constexpr size_t max_depth = static_cast(std::numeric_limits::max()); qos_profile_.depth = max_depth; - EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); EXPECT_LE(depth, static_cast(subscriber_qos_.history().depth)); if (max_depth < std::numeric_limits::max()) { qos_profile_.depth = max_depth + 1; - EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); } } @@ -137,28 +137,28 @@ class GetDataWriterQoSTest : public ::testing::Test TEST_F(GetDataWriterQoSTest, test_unknown_history_policy_conversion_fails) { qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_reliability_policy_conversion_fails) { qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_durability_policy_conversion_fails) { qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_liveliness_policy_conversion_fails) { qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } @@ -176,7 +176,7 @@ TEST_F(GetDataWriterQoSTest, nominal_conversion) { qos_profile_.liveliness_lease_duration.sec = 10u; qos_profile_.liveliness_lease_duration.nsec = 0u; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); EXPECT_EQ( eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, @@ -204,7 +204,7 @@ TEST_F(GetDataWriterQoSTest, large_depth_conversion) { qos_profile_.depth = depth; qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); EXPECT_EQ( eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, @@ -215,12 +215,12 @@ TEST_F(GetDataWriterQoSTest, large_depth_conversion) { constexpr size_t max_depth = static_cast(std::numeric_limits::max()); qos_profile_.depth = max_depth; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); EXPECT_LE(depth, static_cast(publisher_qos_.history().depth)); if (max_depth < std::numeric_limits::max()) { qos_profile_.depth = max_depth + 1; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); } } @@ -229,7 +229,7 @@ TEST_F(GetDataReaderQoSTest, infinite_duration_conversions) qos_profile_.lifespan = RMW_DURATION_INFINITE; qos_profile_.deadline = RMW_DURATION_INFINITE; qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; - EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); EXPECT_EQ(subscriber_qos_.lifespan().duration, InfiniteDuration); EXPECT_EQ(subscriber_qos_.deadline().period, InfiniteDuration); EXPECT_EQ(subscriber_qos_.liveliness().lease_duration, InfiniteDuration); @@ -240,7 +240,7 @@ TEST_F(GetDataWriterQoSTest, infinite_duration_conversions) qos_profile_.lifespan = RMW_DURATION_INFINITE; qos_profile_.deadline = RMW_DURATION_INFINITE; qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); EXPECT_EQ(publisher_qos_.lifespan().duration, InfiniteDuration); EXPECT_EQ(publisher_qos_.deadline().period, InfiniteDuration); EXPECT_EQ(publisher_qos_.liveliness().lease_duration, InfiniteDuration); From 3e9feea262c6508922249b80a4784cef5a6014c7 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 16 Feb 2023 22:29:32 -0800 Subject: [PATCH 04/17] Receive type hash (and debug print) Signed-off-by: Emerson Knapp --- .../custom_participant_info.hpp | 32 ++++++++++++++++--- 1 file changed, 27 insertions(+), 5 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 0e695c9de6..364854c136 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -209,11 +209,33 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList memset(type_hash, 0x2c, RCUTILS_SHA256_BLOCK_SIZE); const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); - std::string user_data(userDataValue.begin(), userDataValue.end()); - RCUTILS_LOG_ERROR("Discovery: %s @ %s --- %s", - proxyData.topicName().to_string().c_str(), - proxyData.typeName().to_string().c_str(), - user_data.c_str()); + const std::string prefix = "type_hash="; + const size_t expected_size = prefix.size() + RCUTILS_SHA256_BLOCK_SIZE + 1; // final ; + if (userDataValue.size() >= expected_size) { + if (memcmp(prefix.data(), userDataValue.data(), prefix.size()) == 0) { + memcpy(type_hash, &userDataValue[prefix.size()], RCUTILS_SHA256_BLOCK_SIZE); + assert(userDataValue[prefix.size() + RCUTILS_SHA256_BLOCK_SIZE] == ';'); + } else { + // TODO(emersonknapp) some other key?? + throw std::runtime_error("Received unexpected USER_DATA key."); + } + } else { + // TODO(emersonknapp) no hash received. + throw std::runtime_error("No hash received in USER_DATA."); + } + + { + // Just debugging output + std::stringstream ss; + ss << std::setw(2) << std::setfill('0'); + for (size_t i = 0; i < RCUTILS_SHA256_BLOCK_SIZE; i++) { + ss << std::hex << static_cast(type_hash[i]); + } + std::string type_hash_str = ss.str(); + RCUTILS_LOG_ERROR("Discovery: %s --- %s", + proxyData.typeName().to_string().c_str(), + type_hash_str.c_str()); + } context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( From 6d38682b42cf103c6f28db12ce0284837e466927 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 20 Feb 2023 11:49:49 -0800 Subject: [PATCH 05/17] Use common helpers for type hash user_data encoding Signed-off-by: Emerson Knapp --- .../custom_participant_info.hpp | 33 ++++--------------- rmw_fastrtps_shared_cpp/src/qos.cpp | 9 ++--- 2 files changed, 9 insertions(+), 33 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 364854c136..fd54c7b82f 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -40,6 +40,7 @@ #include "rmw/rmw.h" #include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/qos.hpp" #include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" #include "rmw_fastrtps_shared_cpp/custom_event_info.hpp" @@ -206,35 +207,13 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); uint8_t type_hash[RCUTILS_SHA256_BLOCK_SIZE]; - memset(type_hash, 0x2c, RCUTILS_SHA256_BLOCK_SIZE); - const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); - const std::string prefix = "type_hash="; - const size_t expected_size = prefix.size() + RCUTILS_SHA256_BLOCK_SIZE + 1; // final ; - if (userDataValue.size() >= expected_size) { - if (memcmp(prefix.data(), userDataValue.data(), prefix.size()) == 0) { - memcpy(type_hash, &userDataValue[prefix.size()], RCUTILS_SHA256_BLOCK_SIZE); - assert(userDataValue[prefix.size() + RCUTILS_SHA256_BLOCK_SIZE] == ';'); - } else { - // TODO(emersonknapp) some other key?? - throw std::runtime_error("Received unexpected USER_DATA key."); - } - } else { - // TODO(emersonknapp) no hash received. - throw std::runtime_error("No hash received in USER_DATA."); - } - + RCUTILS_LOG_ERROR("Discovery: %s", proxyData.typeName().to_string().c_str()); + if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data_qos( + userDataValue.data(), userDataValue.size(), type_hash)) { - // Just debugging output - std::stringstream ss; - ss << std::setw(2) << std::setfill('0'); - for (size_t i = 0; i < RCUTILS_SHA256_BLOCK_SIZE; i++) { - ss << std::hex << static_cast(type_hash[i]); - } - std::string type_hash_str = ss.str(); - RCUTILS_LOG_ERROR("Discovery: %s --- %s", - proxyData.typeName().to_string().c_str(), - type_hash_str.c_str()); + // TODO(emersonknapp) this should be handled OK because we won't always receive it + throw std::runtime_error("Type hash not received."); } context->graph_cache.add_entity( diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 47c59ba2a2..dcff78bc67 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -21,6 +21,7 @@ #include "fastdds/dds/topic/qos/TopicQos.hpp" #include "rmw/error_handling.h" +#include "rmw_dds_common/qos.hpp" #include "time_utils.hpp" @@ -151,14 +152,10 @@ bool fill_data_entity_qos_from_profile( if (!fill_entity_qos_from_profile(qos_policies, entity_qos)) { return false; } - - std::string prefix = "type_hash="; - std::vector user_data(prefix.begin(), prefix.end()); - user_data.insert(user_data.end(), type_hash, type_hash + 32); - user_data.push_back(';'); + std::vector user_data; + rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, user_data); entity_qos.user_data().resize(user_data.size()); entity_qos.user_data().setValue(user_data); - return true; } From 0b598a025ce30b7412681d6b7fd2efd55252a820 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 20 Feb 2023 16:29:34 -0800 Subject: [PATCH 06/17] User data key-value strings Signed-off-by: Emerson Knapp --- rmw_fastrtps_shared_cpp/src/qos.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index dcff78bc67..b28e7de181 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -152,8 +152,8 @@ bool fill_data_entity_qos_from_profile( if (!fill_entity_qos_from_profile(qos_policies, entity_qos)) { return false; } - std::vector user_data; - rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, user_data); + auto user_data_str = rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash); + std::vector user_data(user_data_str.begin(), user_data_str.end()); entity_qos.user_data().resize(user_data.size()); entity_qos.user_data().setValue(user_data); return true; From 41fe673ae60df5445a1167c0c7e1a627da829dbe Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 21 Feb 2023 21:35:39 -0800 Subject: [PATCH 07/17] Type hash struct first pass Signed-off-by: Emerson Knapp --- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 4 +- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 7 +++- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 7 +++- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 4 +- .../custom_participant_info.hpp | 9 +---- .../include/rmw_fastrtps_shared_cpp/qos.hpp | 6 ++- rmw_fastrtps_shared_cpp/src/qos.cpp | 6 +-- .../test/test_rmw_qos_to_dds_attributes.cpp | 38 ++++++++++--------- 8 files changed, 45 insertions(+), 36 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 0472fdef31..5da2e34bb7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -262,7 +262,9 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } // Get QoS from RMW - if (!get_datawriter_qos(*qos_policies, nullptr, writer_qos)) { + // TODO(emersonknapp) + const auto type_hash = rosidl_get_zero_initialized_type_hash(); + if (!get_datawriter_qos(*qos_policies, type_hash, writer_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 6055b21287..d8009c29ad 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -345,7 +345,9 @@ rmw_create_client( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, nullptr, reader_qos)) { + const auto type_hash = rosidl_get_zero_initialized_type_hash(); + // TODO(emersonknapp) not zero-init typehash + if (!get_datareader_qos(adapted_qos_policies, type_hash, reader_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -399,7 +401,8 @@ rmw_create_client( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, nullptr, writer_qos)) { + // TODO(emersonknapp) not zero-init typehash + if (!get_datawriter_qos(adapted_qos_policies, type_hash, writer_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index b70fd8e943..6c58035c36 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -344,7 +344,9 @@ rmw_create_service( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, nullptr, reader_qos)) { + auto type_hash = rosidl_get_zero_initialized_type_hash(); + // TODO(emersonknapp) not zero-init typehash + if (!get_datareader_qos(adapted_qos_policies, type_hash, reader_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -402,7 +404,8 @@ rmw_create_service( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, nullptr, writer_qos)) { + // TODO(emersonknapp) not zero-init typehash + if (!get_datawriter_qos(adapted_qos_policies, type_hash, writer_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 391cef53f0..e4ab770c22 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -264,7 +264,9 @@ create_subscription( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(*qos_policies, nullptr, reader_qos)) { + // TODO(emersonknapp) + const auto type_hash = rosidl_get_zero_initialized_type_hash(); + if (!get_datareader_qos(*qos_policies, type_hash, reader_qos)) { RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; } diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index fd54c7b82f..7aa43bf724 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -206,15 +206,10 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); - uint8_t type_hash[RCUTILS_SHA256_BLOCK_SIZE]; const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); RCUTILS_LOG_ERROR("Discovery: %s", proxyData.typeName().to_string().c_str()); - if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data_qos( - userDataValue.data(), userDataValue.size(), type_hash)) - { - // TODO(emersonknapp) this should be handled OK because we won't always receive it - throw std::runtime_error("Type hash not received."); - } + const auto type_hash = rmw_dds_common::parse_type_hash_from_user_data_qos( + userDataValue.data(), userDataValue.size()); context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index 791fb251ab..694745b35a 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -27,6 +27,8 @@ #include "rmw_fastrtps_shared_cpp/visibility_control.h" +#include "rosidl_runtime_c/type_hash.h" + RMW_FASTRTPS_SHARED_CPP_PUBLIC bool is_valid_qos(const rmw_qos_profile_t & qos_policies); @@ -35,14 +37,14 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, - const uint8_t * type_hash, + const rosidl_type_hash_t & type_hash, eprosima::fastdds::dds::DataReaderQos & reader_qos); RMW_FASTRTPS_SHARED_CPP_PUBLIC bool get_datawriter_qos( const rmw_qos_profile_t & qos_policies, - const uint8_t * type_hash, + const rosidl_type_hash_t & type_hash, eprosima::fastdds::dds::DataWriterQos & writer_qos); RMW_FASTRTPS_SHARED_CPP_PUBLIC diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index b28e7de181..6230ce3c88 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -146,7 +146,7 @@ bool fill_entity_qos_from_profile( template bool fill_data_entity_qos_from_profile( const rmw_qos_profile_t & qos_policies, - const uint8_t * type_hash, + const rosidl_type_hash_t & type_hash, DDSEntityQos & entity_qos) { if (!fill_entity_qos_from_profile(qos_policies, entity_qos)) { @@ -162,7 +162,7 @@ bool fill_data_entity_qos_from_profile( bool get_datareader_qos( const rmw_qos_profile_t & qos_policies, - const uint8_t * type_hash, + const rosidl_type_hash_t & type_hash, eprosima::fastdds::dds::DataReaderQos & datareader_qos) { return fill_data_entity_qos_from_profile(qos_policies, type_hash, datareader_qos); @@ -171,7 +171,7 @@ get_datareader_qos( bool get_datawriter_qos( const rmw_qos_profile_t & qos_policies, - const uint8_t * type_hash, + const rosidl_type_hash_t & type_hash, eprosima::fastdds::dds::DataWriterQos & datawriter_qos) { return fill_data_entity_qos_from_profile(qos_policies, type_hash, datawriter_qos); diff --git a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp index 825991431b..7aedc8d4a7 100644 --- a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp @@ -30,6 +30,8 @@ using eprosima::fastdds::dds::DataReaderQos; static const eprosima::fastrtps::Duration_t InfiniteDuration = eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); +static const rosidl_type_hash_t ZeroTypeHash = rosidl_get_zero_initialized_type_hash(); + class GetDataReaderQoSTest : public ::testing::Test { protected: @@ -39,28 +41,28 @@ class GetDataReaderQoSTest : public ::testing::Test TEST_F(GetDataReaderQoSTest, test_unknown_history_policy_conversion_fails) { qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_reliability_policy_conversion_fails) { qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_durability_policy_conversion_fails) { qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_liveliness_policy_conversion_fails) { qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } @@ -78,7 +80,7 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { qos_profile_.liveliness_lease_duration.sec = 10u; qos_profile_.liveliness_lease_duration.nsec = 0u; - EXPECT_TRUE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_EQ( eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, @@ -106,7 +108,7 @@ TEST_F(GetDataReaderQoSTest, large_depth_conversion) { qos_profile_.depth = depth; qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - EXPECT_TRUE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_EQ( eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, @@ -117,12 +119,12 @@ TEST_F(GetDataReaderQoSTest, large_depth_conversion) { constexpr size_t max_depth = static_cast(std::numeric_limits::max()); qos_profile_.depth = max_depth; - EXPECT_TRUE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_LE(depth, static_cast(subscriber_qos_.history().depth)); if (max_depth < std::numeric_limits::max()) { qos_profile_.depth = max_depth + 1; - EXPECT_FALSE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); } } @@ -137,28 +139,28 @@ class GetDataWriterQoSTest : public ::testing::Test TEST_F(GetDataWriterQoSTest, test_unknown_history_policy_conversion_fails) { qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_reliability_policy_conversion_fails) { qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_durability_policy_conversion_fails) { qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_liveliness_policy_conversion_fails) { qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } @@ -176,7 +178,7 @@ TEST_F(GetDataWriterQoSTest, nominal_conversion) { qos_profile_.liveliness_lease_duration.sec = 10u; qos_profile_.liveliness_lease_duration.nsec = 0u; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); EXPECT_EQ( eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, @@ -204,7 +206,7 @@ TEST_F(GetDataWriterQoSTest, large_depth_conversion) { qos_profile_.depth = depth; qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); EXPECT_EQ( eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, @@ -215,12 +217,12 @@ TEST_F(GetDataWriterQoSTest, large_depth_conversion) { constexpr size_t max_depth = static_cast(std::numeric_limits::max()); qos_profile_.depth = max_depth; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); EXPECT_LE(depth, static_cast(publisher_qos_.history().depth)); if (max_depth < std::numeric_limits::max()) { qos_profile_.depth = max_depth + 1; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); } } @@ -229,7 +231,7 @@ TEST_F(GetDataReaderQoSTest, infinite_duration_conversions) qos_profile_.lifespan = RMW_DURATION_INFINITE; qos_profile_.deadline = RMW_DURATION_INFINITE; qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; - EXPECT_TRUE(get_datareader_qos(qos_profile_, nullptr, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_EQ(subscriber_qos_.lifespan().duration, InfiniteDuration); EXPECT_EQ(subscriber_qos_.deadline().period, InfiniteDuration); EXPECT_EQ(subscriber_qos_.liveliness().lease_duration, InfiniteDuration); @@ -240,7 +242,7 @@ TEST_F(GetDataWriterQoSTest, infinite_duration_conversions) qos_profile_.lifespan = RMW_DURATION_INFINITE; qos_profile_.deadline = RMW_DURATION_INFINITE; qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, nullptr, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); EXPECT_EQ(publisher_qos_.lifespan().duration, InfiniteDuration); EXPECT_EQ(publisher_qos_.deadline().period, InfiniteDuration); EXPECT_EQ(publisher_qos_.liveliness().lease_duration, InfiniteDuration); From 04788cda9f5268c4a57a4d947c24e5fbaf815a15 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 21 Feb 2023 23:29:01 -0800 Subject: [PATCH 08/17] Fix debug print Signed-off-by: Emerson Knapp --- .../include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 7aa43bf724..543ff4e76a 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -208,6 +208,7 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); RCUTILS_LOG_ERROR("Discovery: %s", proxyData.typeName().to_string().c_str()); + RCUTILS_LOG_ERROR(" -- %*s", int(userDataValue.size()), userDataValue.data()); const auto type_hash = rmw_dds_common::parse_type_hash_from_user_data_qos( userDataValue.data(), userDataValue.size()); From 765021c5b5ba36580928065778c583f3a66c481e Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 23 Feb 2023 13:15:31 -0800 Subject: [PATCH 09/17] Add dynamic_cpp implementations Signed-off-by: Emerson Knapp --- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 15 ++++++-- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 37 +++++++++++-------- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 34 +++++++++++------ rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 15 ++++++-- 4 files changed, 66 insertions(+), 35 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 5da2e34bb7..4bc321eb9d 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -102,14 +102,23 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Get RMW Type Support + auto type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) { + if (type_support) { + auto members = static_cast( + type_support->data); + type_hash = members->type_hash_; + } else { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) { + if (type_support) { + auto members = static_cast( + type_support->data); + type_hash = members->type_hash_; + } else { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -262,8 +271,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } // Get QoS from RMW - // TODO(emersonknapp) - const auto type_hash = rosidl_get_zero_initialized_type_hash(); if (!get_datawriter_qos(*qos_policies, type_hash, writer_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index d8009c29ad..e692f4c0da 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -115,14 +115,32 @@ rmw_create_client( ///// // Get RMW Type Support + const void * untyped_request_members; + const void * untyped_response_members; + rosidl_type_hash_t request_type_hash = rosidl_get_zero_initialized_type_hash(); + rosidl_type_hash_t response_type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) { + if (type_support) { + auto service_members = static_cast( + type_support->data); + request_type_hash = service_members->request_members_->type_hash_; + response_type_hash = service_members->response_members_->type_hash_; + untyped_request_members = service_members->request_members_; + untyped_response_members = service_members->response_members_; + } else { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) { + if (type_support) { + auto service_members = static_cast( + type_support->data); + request_type_hash = service_members->request_members_->type_hash_; + response_type_hash = service_members->response_members_->type_hash_; + untyped_request_members = service_members->request_members_; + untyped_response_members = service_members->response_members_; + } else { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -141,14 +159,6 @@ rmw_create_client( // Find and check existing topics and types // Create Topic and Type names - const void * untyped_request_members; - const void * untyped_response_members; - - untyped_request_members = get_request_ptr( - type_support->data, type_support->typesupport_identifier); - untyped_response_members = get_response_ptr( - type_support->data, type_support->typesupport_identifier); - std::string request_type_name = _create_type_name( untyped_request_members, type_support->typesupport_identifier); std::string response_type_name = _create_type_name( @@ -345,9 +355,7 @@ rmw_create_client( reader_qos.data_sharing().off(); } - const auto type_hash = rosidl_get_zero_initialized_type_hash(); - // TODO(emersonknapp) not zero-init typehash - if (!get_datareader_qos(adapted_qos_policies, type_hash, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, response_type_hash, reader_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -401,8 +409,7 @@ rmw_create_client( writer_qos.data_sharing().off(); } - // TODO(emersonknapp) not zero-init typehash - if (!get_datawriter_qos(adapted_qos_policies, type_hash, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, request_type_hash, writer_qos)) { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 6c58035c36..ea7730b8a7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -123,14 +123,32 @@ rmw_create_service( ///// // Get RMW Type Support + const void * untyped_request_members; + const void * untyped_response_members; + rosidl_type_hash_t request_type_hash = rosidl_get_zero_initialized_type_hash(); + rosidl_type_hash_t response_type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) { + if (type_support) { + auto service_members = static_cast( + type_support->data); + request_type_hash = service_members->request_members_->type_hash_; + response_type_hash = service_members->response_members_->type_hash_; + untyped_request_members = service_members->request_members_; + untyped_response_members = service_members->response_members_; + } else { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) { + if (type_support) { + auto service_members = static_cast( + type_support->data); + request_type_hash = service_members->request_members_->type_hash_; + response_type_hash = service_members->response_members_->type_hash_; + untyped_request_members = service_members->request_members_; + untyped_response_members = service_members->response_members_; + } else { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -149,11 +167,6 @@ rmw_create_service( // Find and check existing topics and types // Create Topic and Type names - const void * untyped_request_members = get_request_ptr( - type_support->data, type_support->typesupport_identifier); - const void * untyped_response_members = get_response_ptr( - type_support->data, type_support->typesupport_identifier); - std::string request_type_name = _create_type_name( untyped_request_members, type_support->typesupport_identifier); std::string response_type_name = _create_type_name( @@ -344,9 +357,7 @@ rmw_create_service( reader_qos.data_sharing().off(); } - auto type_hash = rosidl_get_zero_initialized_type_hash(); - // TODO(emersonknapp) not zero-init typehash - if (!get_datareader_qos(adapted_qos_policies, type_hash, reader_qos)) { + if (!get_datareader_qos(adapted_qos_policies, request_type_hash, reader_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -404,8 +415,7 @@ rmw_create_service( writer_qos.data_sharing().off(); } - // TODO(emersonknapp) not zero-init typehash - if (!get_datawriter_qos(adapted_qos_policies, type_hash, writer_qos)) { + if (!get_datawriter_qos(adapted_qos_policies, response_type_hash, writer_qos)) { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index e4ab770c22..7e444f0371 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -103,14 +103,23 @@ create_subscription( ///// // Get RMW Type Support + auto type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) { + if (type_support) { + auto members = static_cast( + type_support->data); + type_hash = members->type_hash_; + } else { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) { + if (type_support) { + auto members = static_cast( + type_support->data); + type_hash = members->type_hash_; + } else { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -264,8 +273,6 @@ create_subscription( reader_qos.data_sharing().off(); } - // TODO(emersonknapp) - const auto type_hash = rosidl_get_zero_initialized_type_hash(); if (!get_datareader_qos(*qos_policies, type_hash, reader_qos)) { RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; From 740421f833b6136d3d75d9f51dcdc958d0c8e254 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 23 Feb 2023 13:47:04 -0800 Subject: [PATCH 10/17] Remove debug logging Signed-off-by: Emerson Knapp --- .../include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 543ff4e76a..b946461649 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -207,8 +207,6 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); - RCUTILS_LOG_ERROR("Discovery: %s", proxyData.typeName().to_string().c_str()); - RCUTILS_LOG_ERROR(" -- %*s", int(userDataValue.size()), userDataValue.data()); const auto type_hash = rmw_dds_common::parse_type_hash_from_user_data_qos( userDataValue.data(), userDataValue.size()); From 1112a0ff8bf879ba9b30a5a8a6ce4a942af65def Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Sun, 12 Mar 2023 20:57:18 -0700 Subject: [PATCH 11/17] Update fn name Signed-off-by: Emerson Knapp --- .../include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index b946461649..609beb6724 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -207,7 +207,7 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); - const auto type_hash = rmw_dds_common::parse_type_hash_from_user_data_qos( + const auto type_hash = rmw_dds_common::parse_type_hash_from_user_data( userDataValue.data(), userDataValue.size()); context->graph_cache.add_entity( From fba5d6069189e259b4b1b4dda170b87a0f31e4a3 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 13 Mar 2023 00:09:32 -0700 Subject: [PATCH 12/17] Fix linter Signed-off-by: Emerson Knapp --- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 16 ++++++++-------- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 16 ++++++++-------- .../test/test_rmw_qos_to_dds_attributes.cpp | 16 ++++++++-------- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index e692f4c0da..20648d929a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -122,20 +122,20 @@ rmw_create_client( const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); if (type_support) { - auto service_members = static_cast( - type_support->data); - request_type_hash = service_members->request_members_->type_hash_; - response_type_hash = service_members->response_members_->type_hash_; - untyped_request_members = service_members->request_members_; - untyped_response_members = service_members->response_members_; + auto service_members = static_cast< + const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); + request_type_hash = service_members->request_members_->type_hash_; + response_type_hash = service_members->response_members_->type_hash_; + untyped_request_members = service_members->request_members_; + untyped_response_members = service_members->response_members_; } else { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); if (type_support) { - auto service_members = static_cast( - type_support->data); + auto service_members = static_cast< + const rosidl_typesupport_introspection_cpp::ServiceMembers *>(type_support->data); request_type_hash = service_members->request_members_->type_hash_; response_type_hash = service_members->response_members_->type_hash_; untyped_request_members = service_members->request_members_; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index ea7730b8a7..9263d70846 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -130,20 +130,20 @@ rmw_create_service( const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); if (type_support) { - auto service_members = static_cast( - type_support->data); - request_type_hash = service_members->request_members_->type_hash_; - response_type_hash = service_members->response_members_->type_hash_; - untyped_request_members = service_members->request_members_; - untyped_response_members = service_members->response_members_; + auto service_members = static_cast< + const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); + request_type_hash = service_members->request_members_->type_hash_; + response_type_hash = service_members->response_members_->type_hash_; + untyped_request_members = service_members->request_members_; + untyped_response_members = service_members->response_members_; } else { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); if (type_support) { - auto service_members = static_cast( - type_support->data); + auto service_members = static_cast< + const rosidl_typesupport_introspection_cpp::ServiceMembers *>(type_support->data); request_type_hash = service_members->request_members_->type_hash_; response_type_hash = service_members->response_members_->type_hash_; untyped_request_members = service_members->request_members_; diff --git a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp index 7aedc8d4a7..0ca4409bdf 100644 --- a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp @@ -41,28 +41,28 @@ class GetDataReaderQoSTest : public ::testing::Test TEST_F(GetDataReaderQoSTest, test_unknown_history_policy_conversion_fails) { qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_reliability_policy_conversion_fails) { qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_durability_policy_conversion_fails) { qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_liveliness_policy_conversion_fails) { qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } @@ -80,7 +80,7 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { qos_profile_.liveliness_lease_duration.sec = 10u; qos_profile_.liveliness_lease_duration.nsec = 0u; - EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_EQ( eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, @@ -108,7 +108,7 @@ TEST_F(GetDataReaderQoSTest, large_depth_conversion) { qos_profile_.depth = depth; qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_EQ( eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, @@ -119,7 +119,7 @@ TEST_F(GetDataReaderQoSTest, large_depth_conversion) { constexpr size_t max_depth = static_cast(std::numeric_limits::max()); qos_profile_.depth = max_depth; - EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_LE(depth, static_cast(subscriber_qos_.history().depth)); if (max_depth < std::numeric_limits::max()) { @@ -231,7 +231,7 @@ TEST_F(GetDataReaderQoSTest, infinite_duration_conversions) qos_profile_.lifespan = RMW_DURATION_INFINITE; qos_profile_.deadline = RMW_DURATION_INFINITE; qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; - EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); EXPECT_EQ(subscriber_qos_.lifespan().duration, InfiniteDuration); EXPECT_EQ(subscriber_qos_.deadline().period, InfiniteDuration); EXPECT_EQ(subscriber_qos_.liveliness().lease_duration, InfiniteDuration); From f22e40046db81321bcf251d9c3d5d8fe7828992f Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 15 Mar 2023 12:00:29 -0700 Subject: [PATCH 13/17] Use new signatures Signed-off-by: Emerson Knapp --- .../rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 8 ++++++-- rmw_fastrtps_shared_cpp/src/qos.cpp | 6 +++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 609beb6724..6d059a9c65 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -207,8 +207,12 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); - const auto type_hash = rmw_dds_common::parse_type_hash_from_user_data( - userDataValue.data(), userDataValue.size()); + rosidl_type_hash_t type_hash; + rmw_ret_t ret = rmw_dds_common::parse_type_hash_from_user_data( + userDataValue.data(), userDataValue.size(), type_hash); + if (ret != RMW_RET_OK) { + type_hash = rosidl_get_zero_initialized_type_hash(); + } context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 6230ce3c88..b4711f16c1 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -152,7 +152,11 @@ bool fill_data_entity_qos_from_profile( if (!fill_entity_qos_from_profile(qos_policies, entity_qos)) { return false; } - auto user_data_str = rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash); + std::string user_data_str; + rmw_ret_t ret = rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, user_data_str); + if (ret != RMW_RET_OK) { + user_data_str.clear(); + } std::vector user_data(user_data_str.begin(), user_data_str.end()); entity_qos.user_data().resize(user_data.size()); entity_qos.user_data().setValue(user_data); From a39da37a05a9a863d8bf963ac7ec722c54c7b36a Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 15 Mar 2023 18:30:00 -0700 Subject: [PATCH 14/17] Address review comments Signed-off-by: Emerson Knapp --- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 4 +++- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 2 ++ rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 2 ++ rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 4 +++- rmw_fastrtps_shared_cpp/CMakeLists.txt | 1 + .../rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 6 +++--- rmw_fastrtps_shared_cpp/package.xml | 1 + rmw_fastrtps_shared_cpp/src/qos.cpp | 7 +++++-- .../test/test_rmw_qos_to_dds_attributes.cpp | 2 ++ 9 files changed, 22 insertions(+), 7 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 4bc321eb9d..b4c021a4d6 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -33,6 +33,8 @@ #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" +#include "rosidl_runtime_c/type_hash.h" + #include "rcpputils/scope_exit.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" @@ -102,7 +104,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Get RMW Type Support - auto type_hash = rosidl_get_zero_initialized_type_hash(); + rosidl_type_hash_t type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); if (type_support) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 20648d929a..252399a9f1 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -38,6 +38,8 @@ #include "rmw_dds_common/qos.hpp" +#include "rosidl_runtime_c/type_hash.h" + #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_c/identifier.h" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 9263d70846..60ac9a6af9 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -48,6 +48,8 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rosidl_runtime_c/type_hash.h" + #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_c/identifier.h" diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 7e444f0371..7962d5a082 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -32,6 +32,8 @@ #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" +#include "rosidl_runtime_c/type_hash.h" + #include "rcpputils/scope_exit.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" @@ -103,7 +105,7 @@ create_subscription( ///// // Get RMW Type Support - auto type_hash = rosidl_get_zero_initialized_type_hash(); + rosidl_type_hash_t type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); if (type_support) { diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index 7c5b196dc6..6890a95199 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -39,6 +39,7 @@ find_package(ament_cmake_ros REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw_dds_common REQUIRED) +find_package(rosidl_runtime_c REQUIRED) find_package(rosidl_typesupport_introspection_c REQUIRED) find_package(rosidl_typesupport_introspection_cpp REQUIRED) find_package(tracetools REQUIRED) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 6d059a9c65..e41bf52fb1 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -208,9 +208,9 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList const auto & userDataValue = proxyData.m_qos.m_userData.getValue(); rosidl_type_hash_t type_hash; - rmw_ret_t ret = rmw_dds_common::parse_type_hash_from_user_data( - userDataValue.data(), userDataValue.size(), type_hash); - if (ret != RMW_RET_OK) { + if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data( + userDataValue.data(), userDataValue.size(), type_hash)) + { type_hash = rosidl_get_zero_initialized_type_hash(); } diff --git a/rmw_fastrtps_shared_cpp/package.xml b/rmw_fastrtps_shared_cpp/package.xml index 5a04ea49ed..e0b5dc3c8c 100644 --- a/rmw_fastrtps_shared_cpp/package.xml +++ b/rmw_fastrtps_shared_cpp/package.xml @@ -27,6 +27,7 @@ rcutils rmw rmw_dds_common + rosidl_runtime_c rosidl_typesupport_introspection_c rosidl_typesupport_introspection_cpp tracetools diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index b4711f16c1..fb2e7b11c4 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "rmw_fastrtps_shared_cpp/qos.hpp" @@ -21,8 +22,11 @@ #include "fastdds/dds/topic/qos/TopicQos.hpp" #include "rmw/error_handling.h" + #include "rmw_dds_common/qos.hpp" +#include "rosidl_runtime_c/type_hash.h" + #include "time_utils.hpp" static @@ -153,8 +157,7 @@ bool fill_data_entity_qos_from_profile( return false; } std::string user_data_str; - rmw_ret_t ret = rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, user_data_str); - if (ret != RMW_RET_OK) { + if (RMW_RET_OK != rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, user_data_str)) { user_data_str.clear(); } std::vector user_data(user_data_str.begin(), user_data_str.end()); diff --git a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp index 0ca4409bdf..2d8da29ba4 100644 --- a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp @@ -25,6 +25,8 @@ #include "rmw/error_handling.h" +#include "rosidl_runtime_c/type_hash.h" + using eprosima::fastdds::dds::DataReaderQos; static const eprosima::fastrtps::Duration_t InfiniteDuration = From f7239204f427457707f67bb0b9fe434ae38b97a2 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 16 Mar 2023 18:30:52 -0700 Subject: [PATCH 15/17] Add warnings on failed type hash operations Signed-off-by: Emerson Knapp --- .../rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 7 +++++++ rmw_fastrtps_shared_cpp/src/qos.cpp | 5 +++++ 2 files changed, 12 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index e41bf52fb1..36b7103014 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -211,6 +211,13 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data( userDataValue.data(), userDataValue.size(), type_hash)) { + RCUTILS_LOG_WARN_NAMED( + "rmw_fastrtps_shared_cpp", + "Failed to parse type hash for topic '%s' with type '%s' from USER_DATA '%*s'.", + proxyData.topicName().c_str(), + proxyData.typeName().c_str(), + static_cast(userDataValue.size()), + reinterpret_cast(userDataValue.data())); type_hash = rosidl_get_zero_initialized_type_hash(); } diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index fb2e7b11c4..bee375c3fe 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -15,6 +15,8 @@ #include #include +#include "rcutils/logging_macros.h" + #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "fastdds/dds/publisher/qos/DataWriterQos.hpp" @@ -158,6 +160,9 @@ bool fill_data_entity_qos_from_profile( } std::string user_data_str; if (RMW_RET_OK != rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, user_data_str)) { + RCUTILS_LOG_WARN_NAMED( + "rmw_fastrtps_shared_cpp", + "Failed to encode type hash for topic, will not distribute it in USER_DATA."); user_data_str.clear(); } std::vector user_data(user_data_str.begin(), user_data_str.end()); From f7b2aab3b9567d41141fcc7ca1eb804ea358627d Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 21 Mar 2023 23:16:47 -0700 Subject: [PATCH 16/17] Use new typesupport hashes Signed-off-by: Emerson Knapp --- rmw_fastrtps_cpp/src/publisher.cpp | 2 +- rmw_fastrtps_cpp/src/rmw_client.cpp | 8 +++- rmw_fastrtps_cpp/src/rmw_service.cpp | 8 +++- rmw_fastrtps_cpp/src/subscription.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 17 ++------ rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 40 ++++++++----------- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 37 ++++++----------- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 15 ++----- 8 files changed, 49 insertions(+), 80 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index bb86e6ff11..a93deeb875 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -256,7 +256,7 @@ rmw_fastrtps_cpp::create_publisher( } // Get QoS from RMW - if (!get_datawriter_qos(*qos_policies, callbacks->type_hash_, writer_qos)) { + if (!get_datawriter_qos(*qos_policies, *type_supports->type_hash, writer_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 985c0d3e40..70a126ee0a 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -314,7 +314,9 @@ rmw_create_client( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, response_members->type_hash_, reader_qos)) { + if (!get_datareader_qos( + adapted_qos_policies, *type_supports->response_typesupport->type_hash, reader_qos)) + { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -368,7 +370,9 @@ rmw_create_client( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, request_members->type_hash_, writer_qos)) { + if (!get_datawriter_qos( + adapted_qos_policies, *type_supports->request_typesupport->type_hash, writer_qos)) + { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 1bc813315c..ed8cf4fa0e 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -313,7 +313,9 @@ rmw_create_service( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, request_members->type_hash_, reader_qos)) { + if (!get_datareader_qos( + adapted_qos_policies, *type_supports->request_typesupport->type_hash, reader_qos)) + { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -371,7 +373,9 @@ rmw_create_service( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, response_members->type_hash_, writer_qos)) { + if (!get_datawriter_qos( + adapted_qos_policies, *type_supports->response_typesupport->type_hash, writer_qos)) + { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 36b17905e8..a367c7626c 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -280,7 +280,7 @@ create_subscription( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(*qos_policies, callbacks->type_hash_, reader_qos)) { + if (!get_datareader_qos(*qos_policies, *type_supports->type_hash, reader_qos)) { RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index b4c021a4d6..6aa6f04bc0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -33,8 +33,6 @@ #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" -#include "rosidl_runtime_c/type_hash.h" - #include "rcpputils/scope_exit.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" @@ -104,23 +102,14 @@ rmw_fastrtps_dynamic_cpp::create_publisher( ///// // Get RMW Type Support - rosidl_type_hash_t type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); - if (type_support) { - auto members = static_cast( - type_support->data); - type_hash = members->type_hash_; - } else { + if (!type_support) { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (type_support) { - auto members = static_cast( - type_support->data); - type_hash = members->type_hash_; - } else { + if (!type_support) { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -273,7 +262,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( } // Get QoS from RMW - if (!get_datawriter_qos(*qos_policies, type_hash, writer_qos)) { + if (!get_datawriter_qos(*qos_policies, *type_supports->type_hash, writer_qos)) { RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 252399a9f1..5ba119381e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -38,8 +38,6 @@ #include "rmw_dds_common/qos.hpp" -#include "rosidl_runtime_c/type_hash.h" - #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_c/identifier.h" @@ -117,32 +115,14 @@ rmw_create_client( ///// // Get RMW Type Support - const void * untyped_request_members; - const void * untyped_response_members; - rosidl_type_hash_t request_type_hash = rosidl_get_zero_initialized_type_hash(); - rosidl_type_hash_t response_type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); - if (type_support) { - auto service_members = static_cast< - const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); - request_type_hash = service_members->request_members_->type_hash_; - response_type_hash = service_members->response_members_->type_hash_; - untyped_request_members = service_members->request_members_; - untyped_response_members = service_members->response_members_; - } else { + if (!type_support) { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (type_support) { - auto service_members = static_cast< - const rosidl_typesupport_introspection_cpp::ServiceMembers *>(type_support->data); - request_type_hash = service_members->request_members_->type_hash_; - response_type_hash = service_members->response_members_->type_hash_; - untyped_request_members = service_members->request_members_; - untyped_response_members = service_members->response_members_; - } else { + if (!type_support) { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -161,6 +141,14 @@ rmw_create_client( // Find and check existing topics and types // Create Topic and Type names + const void * untyped_request_members; + const void * untyped_response_members; + + untyped_request_members = get_request_ptr( + type_support->data, type_support->typesupport_identifier); + untyped_response_members = get_response_ptr( + type_support->data, type_support->typesupport_identifier); + std::string request_type_name = _create_type_name( untyped_request_members, type_support->typesupport_identifier); std::string response_type_name = _create_type_name( @@ -357,7 +345,9 @@ rmw_create_client( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, response_type_hash, reader_qos)) { + if (!get_datareader_qos( + adapted_qos_policies, *type_supports->response_typesupport->type_hash, reader_qos)) + { RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); return nullptr; } @@ -411,7 +401,9 @@ rmw_create_client( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, request_type_hash, writer_qos)) { + if (!get_datawriter_qos( + adapted_qos_policies, *type_supports->request_typesupport->type_hash, writer_qos)) + { RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 60ac9a6af9..0ae48d98d3 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -48,8 +48,6 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "rosidl_runtime_c/type_hash.h" - #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_c/identifier.h" @@ -125,32 +123,14 @@ rmw_create_service( ///// // Get RMW Type Support - const void * untyped_request_members; - const void * untyped_response_members; - rosidl_type_hash_t request_type_hash = rosidl_get_zero_initialized_type_hash(); - rosidl_type_hash_t response_type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); - if (type_support) { - auto service_members = static_cast< - const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); - request_type_hash = service_members->request_members_->type_hash_; - response_type_hash = service_members->response_members_->type_hash_; - untyped_request_members = service_members->request_members_; - untyped_response_members = service_members->response_members_; - } else { + if (!type_support) { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_service_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (type_support) { - auto service_members = static_cast< - const rosidl_typesupport_introspection_cpp::ServiceMembers *>(type_support->data); - request_type_hash = service_members->request_members_->type_hash_; - response_type_hash = service_members->response_members_->type_hash_; - untyped_request_members = service_members->request_members_; - untyped_response_members = service_members->response_members_; - } else { + if (!type_support) { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -169,6 +149,11 @@ rmw_create_service( // Find and check existing topics and types // Create Topic and Type names + const void * untyped_request_members = get_request_ptr( + type_support->data, type_support->typesupport_identifier); + const void * untyped_response_members = get_response_ptr( + type_support->data, type_support->typesupport_identifier); + std::string request_type_name = _create_type_name( untyped_request_members, type_support->typesupport_identifier); std::string response_type_name = _create_type_name( @@ -359,7 +344,9 @@ rmw_create_service( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(adapted_qos_policies, request_type_hash, reader_qos)) { + if (!get_datareader_qos( + adapted_qos_policies, *type_supports->request_typesupport->type_hash, reader_qos)) + { RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); return nullptr; } @@ -417,7 +404,9 @@ rmw_create_service( writer_qos.data_sharing().off(); } - if (!get_datawriter_qos(adapted_qos_policies, response_type_hash, writer_qos)) { + if (!get_datawriter_qos( + adapted_qos_policies, *type_supports->response_typesupport->type_hash, writer_qos)) + { RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 7962d5a082..511ffb9a90 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -105,23 +105,14 @@ create_subscription( ///// // Get RMW Type Support - rosidl_type_hash_t type_hash = rosidl_get_zero_initialized_type_hash(); const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); - if (type_support) { - auto members = static_cast( - type_support->data); - type_hash = members->type_hash_; - } else { + if (!type_support) { rcutils_error_string_t prev_error_string = rcutils_get_error_string(); rcutils_reset_error(); type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (type_support) { - auto members = static_cast( - type_support->data); - type_hash = members->type_hash_; - } else { + if (!type_support) { rcutils_error_string_t error_string = rcutils_get_error_string(); rcutils_reset_error(); RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -275,7 +266,7 @@ create_subscription( reader_qos.data_sharing().off(); } - if (!get_datareader_qos(*qos_policies, type_hash, reader_qos)) { + if (!get_datareader_qos(*qos_policies, *type_supports->type_hash, reader_qos)) { RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); return nullptr; } From 40afb8091fe0503c38949394bda3b80d467a1d05 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 23 Mar 2023 11:28:20 -0700 Subject: [PATCH 17/17] review nits Signed-off-by: Emerson Knapp --- rmw_fastrtps_shared_cpp/src/qos.cpp | 3 +- .../test/test_rmw_qos_to_dds_attributes.cpp | 38 +++++++++---------- 2 files changed, 21 insertions(+), 20 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index bee375c3fe..1adae0bba1 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -150,7 +150,8 @@ bool fill_entity_qos_from_profile( } template -bool fill_data_entity_qos_from_profile( +bool +fill_data_entity_qos_from_profile( const rmw_qos_profile_t & qos_policies, const rosidl_type_hash_t & type_hash, DDSEntityQos & entity_qos) diff --git a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp index 2d8da29ba4..136ec993a6 100644 --- a/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp +++ b/rmw_fastrtps_shared_cpp/test/test_rmw_qos_to_dds_attributes.cpp @@ -32,7 +32,7 @@ using eprosima::fastdds::dds::DataReaderQos; static const eprosima::fastrtps::Duration_t InfiniteDuration = eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); -static const rosidl_type_hash_t ZeroTypeHash = rosidl_get_zero_initialized_type_hash(); +static const rosidl_type_hash_t zero_type_hash = rosidl_get_zero_initialized_type_hash(); class GetDataReaderQoSTest : public ::testing::Test { @@ -43,28 +43,28 @@ class GetDataReaderQoSTest : public ::testing::Test TEST_F(GetDataReaderQoSTest, test_unknown_history_policy_conversion_fails) { qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, zero_type_hash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_reliability_policy_conversion_fails) { qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, zero_type_hash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_durability_policy_conversion_fails) { qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, zero_type_hash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataReaderQoSTest, unknown_liveliness_policy_conversion_fails) { qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, zero_type_hash, subscriber_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } @@ -82,7 +82,7 @@ TEST_F(GetDataReaderQoSTest, nominal_conversion) { qos_profile_.liveliness_lease_duration.sec = 10u; qos_profile_.liveliness_lease_duration.nsec = 0u; - EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, zero_type_hash, subscriber_qos_)); EXPECT_EQ( eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, @@ -110,7 +110,7 @@ TEST_F(GetDataReaderQoSTest, large_depth_conversion) { qos_profile_.depth = depth; qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, zero_type_hash, subscriber_qos_)); EXPECT_EQ( eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, @@ -121,12 +121,12 @@ TEST_F(GetDataReaderQoSTest, large_depth_conversion) { constexpr size_t max_depth = static_cast(std::numeric_limits::max()); qos_profile_.depth = max_depth; - EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, zero_type_hash, subscriber_qos_)); EXPECT_LE(depth, static_cast(subscriber_qos_.history().depth)); if (max_depth < std::numeric_limits::max()) { qos_profile_.depth = max_depth + 1; - EXPECT_FALSE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_FALSE(get_datareader_qos(qos_profile_, zero_type_hash, subscriber_qos_)); } } @@ -141,28 +141,28 @@ class GetDataWriterQoSTest : public ::testing::Test TEST_F(GetDataWriterQoSTest, test_unknown_history_policy_conversion_fails) { qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, zero_type_hash, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_reliability_policy_conversion_fails) { qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, zero_type_hash, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_durability_policy_conversion_fails) { qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, zero_type_hash, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } TEST_F(GetDataWriterQoSTest, unknown_liveliness_policy_conversion_fails) { qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, zero_type_hash, publisher_qos_)); EXPECT_TRUE(rmw_error_is_set()); rmw_reset_error(); } @@ -180,7 +180,7 @@ TEST_F(GetDataWriterQoSTest, nominal_conversion) { qos_profile_.liveliness_lease_duration.sec = 10u; qos_profile_.liveliness_lease_duration.nsec = 0u; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, zero_type_hash, publisher_qos_)); EXPECT_EQ( eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, @@ -208,7 +208,7 @@ TEST_F(GetDataWriterQoSTest, large_depth_conversion) { qos_profile_.depth = depth; qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, zero_type_hash, publisher_qos_)); EXPECT_EQ( eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, @@ -219,12 +219,12 @@ TEST_F(GetDataWriterQoSTest, large_depth_conversion) { constexpr size_t max_depth = static_cast(std::numeric_limits::max()); qos_profile_.depth = max_depth; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, zero_type_hash, publisher_qos_)); EXPECT_LE(depth, static_cast(publisher_qos_.history().depth)); if (max_depth < std::numeric_limits::max()) { qos_profile_.depth = max_depth + 1; - EXPECT_FALSE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); + EXPECT_FALSE(get_datawriter_qos(qos_profile_, zero_type_hash, publisher_qos_)); } } @@ -233,7 +233,7 @@ TEST_F(GetDataReaderQoSTest, infinite_duration_conversions) qos_profile_.lifespan = RMW_DURATION_INFINITE; qos_profile_.deadline = RMW_DURATION_INFINITE; qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; - EXPECT_TRUE(get_datareader_qos(qos_profile_, ZeroTypeHash, subscriber_qos_)); + EXPECT_TRUE(get_datareader_qos(qos_profile_, zero_type_hash, subscriber_qos_)); EXPECT_EQ(subscriber_qos_.lifespan().duration, InfiniteDuration); EXPECT_EQ(subscriber_qos_.deadline().period, InfiniteDuration); EXPECT_EQ(subscriber_qos_.liveliness().lease_duration, InfiniteDuration); @@ -244,7 +244,7 @@ TEST_F(GetDataWriterQoSTest, infinite_duration_conversions) qos_profile_.lifespan = RMW_DURATION_INFINITE; qos_profile_.deadline = RMW_DURATION_INFINITE; qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; - EXPECT_TRUE(get_datawriter_qos(qos_profile_, ZeroTypeHash, publisher_qos_)); + EXPECT_TRUE(get_datawriter_qos(qos_profile_, zero_type_hash, publisher_qos_)); EXPECT_EQ(publisher_qos_.lifespan().duration, InfiniteDuration); EXPECT_EQ(publisher_qos_.deadline().period, InfiniteDuration); EXPECT_EQ(publisher_qos_.liveliness().lease_duration, InfiniteDuration);